jackyliang42 commited on
Commit
9a40e4f
1 Parent(s): 566dbba

working video

Browse files
This view is limited to 50 files because it contains too many changes.   See raw diff
Files changed (50) hide show
  1. .gitattributes +14 -0
  2. .gitignore +162 -0
  3. app.py +143 -0
  4. bowl/bowl.urdf +29 -0
  5. bowl/cup.obj +1413 -0
  6. bowl/textured-0008192.obj +0 -0
  7. cfg.yaml +90 -0
  8. consts.py +33 -0
  9. lmp.py +251 -0
  10. prompts/fgen.py +49 -0
  11. prompts/parse_obj_name.py +64 -0
  12. prompts/parse_position.py +54 -0
  13. prompts/parse_question.py +30 -0
  14. prompts/tabletop_ui.py +198 -0
  15. prompts/transform_shape_pts.py +19 -0
  16. requirements.txt +11 -0
  17. robotiq_2f_85/README.md +52 -0
  18. robotiq_2f_85/robotiq-2f-base.mtl +13 -0
  19. robotiq_2f_85/robotiq-2f-base.obj +0 -0
  20. robotiq_2f_85/robotiq-2f-base.stl +3 -0
  21. robotiq_2f_85/robotiq-2f-coupler.mtl +13 -0
  22. robotiq_2f_85/robotiq-2f-coupler.obj +0 -0
  23. robotiq_2f_85/robotiq-2f-coupler.stl +3 -0
  24. robotiq_2f_85/robotiq-2f-driver.mtl +13 -0
  25. robotiq_2f_85/robotiq-2f-driver.obj +0 -0
  26. robotiq_2f_85/robotiq-2f-driver.stl +3 -0
  27. robotiq_2f_85/robotiq-2f-follower.mtl +13 -0
  28. robotiq_2f_85/robotiq-2f-follower.obj +0 -0
  29. robotiq_2f_85/robotiq-2f-follower.stl +3 -0
  30. robotiq_2f_85/robotiq-2f-pad.stl +3 -0
  31. robotiq_2f_85/robotiq-2f-spring_link.mtl +13 -0
  32. robotiq_2f_85/robotiq-2f-spring_link.obj +0 -0
  33. robotiq_2f_85/robotiq-2f-spring_link.stl +3 -0
  34. robotiq_2f_85/robotiq_2f_85.urdf +299 -0
  35. robotiq_2f_85/textures/gripper-2f_BaseColor.jpg +0 -0
  36. robotiq_2f_85/textures/gripper-2f_Metallic.jpg +0 -0
  37. robotiq_2f_85/textures/gripper-2f_Normal.jpg +0 -0
  38. robotiq_2f_85/textures/gripper-2f_Roughness.jpg +0 -0
  39. sim.py +655 -0
  40. ur5e/collision/base.stl +3 -0
  41. ur5e/collision/forearm.stl +3 -0
  42. ur5e/collision/shoulder.stl +3 -0
  43. ur5e/collision/upperarm.stl +3 -0
  44. ur5e/collision/wrist1.stl +3 -0
  45. ur5e/collision/wrist2.stl +3 -0
  46. ur5e/collision/wrist3.stl +3 -0
  47. ur5e/ur5e.urdf +279 -0
  48. ur5e/visual/base.dae +0 -0
  49. ur5e/visual/forearm.dae +0 -0
  50. ur5e/visual/shoulder.dae +0 -0
.gitattributes CHANGED
@@ -29,3 +29,17 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
29
  *.zip filter=lfs diff=lfs merge=lfs -text
30
  *.zst filter=lfs diff=lfs merge=lfs -text
31
  *tfevents* filter=lfs diff=lfs merge=lfs -text
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
29
  *.zip filter=lfs diff=lfs merge=lfs -text
30
  *.zst filter=lfs diff=lfs merge=lfs -text
31
  *tfevents* filter=lfs diff=lfs merge=lfs -text
32
+ ur5e/collision/wrist2.stl filter=lfs diff=lfs merge=lfs -text
33
+ ur5e/collision/wrist3.stl filter=lfs diff=lfs merge=lfs -text
34
+ ur5e/collision/base.stl filter=lfs diff=lfs merge=lfs -text
35
+ ur5e/collision/forearm.stl filter=lfs diff=lfs merge=lfs -text
36
+ ur5e/collision/shoulder.stl filter=lfs diff=lfs merge=lfs -text
37
+ ur5e/collision/upperarm.stl filter=lfs diff=lfs merge=lfs -text
38
+ ur5e/collision/wrist1.stl filter=lfs diff=lfs merge=lfs -text
39
+ ur5e/visual filter=lfs diff=lfs merge=lfs -text
40
+ robotiq_2f_85/robotiq-2f-pad.stl filter=lfs diff=lfs merge=lfs -text
41
+ robotiq_2f_85/robotiq-2f-spring_link.stl filter=lfs diff=lfs merge=lfs -text
42
+ robotiq_2f_85/robotiq-2f-base.stl filter=lfs diff=lfs merge=lfs -text
43
+ robotiq_2f_85/robotiq-2f-coupler.stl filter=lfs diff=lfs merge=lfs -text
44
+ robotiq_2f_85/robotiq-2f-driver.stl filter=lfs diff=lfs merge=lfs -text
45
+ robotiq_2f_85/robotiq-2f-follower.stl filter=lfs diff=lfs merge=lfs -text
.gitignore ADDED
@@ -0,0 +1,162 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Byte-compiled / optimized / DLL files
2
+ __pycache__/
3
+ *.py[cod]
4
+ *$py.class
5
+
6
+ # C extensions
7
+ *.so
8
+
9
+ # Distribution / packaging
10
+ .Python
11
+ build/
12
+ develop-eggs/
13
+ dist/
14
+ downloads/
15
+ eggs/
16
+ .eggs/
17
+ lib/
18
+ lib64/
19
+ parts/
20
+ sdist/
21
+ var/
22
+ wheels/
23
+ share/python-wheels/
24
+ *.egg-info/
25
+ .installed.cfg
26
+ *.egg
27
+ MANIFEST
28
+
29
+ # PyInstaller
30
+ # Usually these files are written by a python script from a template
31
+ # before PyInstaller builds the exe, so as to inject date/other infos into it.
32
+ *.manifest
33
+ *.spec
34
+
35
+ # Installer logs
36
+ pip-log.txt
37
+ pip-delete-this-directory.txt
38
+
39
+ # Unit test / coverage reports
40
+ htmlcov/
41
+ .tox/
42
+ .nox/
43
+ .coverage
44
+ .coverage.*
45
+ .cache
46
+ nosetests.xml
47
+ coverage.xml
48
+ *.cover
49
+ *.py,cover
50
+ .hypothesis/
51
+ .pytest_cache/
52
+ cover/
53
+
54
+ # Translations
55
+ *.mo
56
+ *.pot
57
+
58
+ # Django stuff:
59
+ *.log
60
+ local_settings.py
61
+ db.sqlite3
62
+ db.sqlite3-journal
63
+
64
+ # Flask stuff:
65
+ instance/
66
+ .webassets-cache
67
+
68
+ # Scrapy stuff:
69
+ .scrapy
70
+
71
+ # Sphinx documentation
72
+ docs/_build/
73
+
74
+ # PyBuilder
75
+ .pybuilder/
76
+ target/
77
+
78
+ # Jupyter Notebook
79
+ .ipynb_checkpoints
80
+
81
+ # IPython
82
+ profile_default/
83
+ ipython_config.py
84
+
85
+ # pyenv
86
+ # For a library or package, you might want to ignore these files since the code is
87
+ # intended to run in multiple environments; otherwise, check them in:
88
+ # .python-version
89
+
90
+ # pipenv
91
+ # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
92
+ # However, in case of collaboration, if having platform-specific dependencies or dependencies
93
+ # having no cross-platform support, pipenv may install dependencies that don't work, or not
94
+ # install all needed dependencies.
95
+ #Pipfile.lock
96
+
97
+ # poetry
98
+ # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
99
+ # This is especially recommended for binary packages to ensure reproducibility, and is more
100
+ # commonly ignored for libraries.
101
+ # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
102
+ #poetry.lock
103
+
104
+ # pdm
105
+ # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
106
+ #pdm.lock
107
+ # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
108
+ # in version control.
109
+ # https://pdm.fming.dev/#use-with-ide
110
+ .pdm.toml
111
+
112
+ # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
113
+ __pypackages__/
114
+
115
+ # Celery stuff
116
+ celerybeat-schedule
117
+ celerybeat.pid
118
+
119
+ # SageMath parsed files
120
+ *.sage.py
121
+
122
+ # Environments
123
+ .env
124
+ .venv
125
+ env/
126
+ venv/
127
+ ENV/
128
+ env.bak/
129
+ venv.bak/
130
+
131
+ # Spyder project settings
132
+ .spyderproject
133
+ .spyproject
134
+
135
+ # Rope project settings
136
+ .ropeproject
137
+
138
+ # mkdocs documentation
139
+ /site
140
+
141
+ # mypy
142
+ .mypy_cache/
143
+ .dmypy.json
144
+ dmypy.json
145
+
146
+ # Pyre type checker
147
+ .pyre/
148
+
149
+ # pytype static type analyzer
150
+ .pytype/
151
+
152
+ # Cython debug symbols
153
+ cython_debug/
154
+
155
+ # PyCharm
156
+ # JetBrains specific template is maintained in a separate JetBrains.gitignore that can
157
+ # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
158
+ # and can be added to the global gitignore or merged into this file. For a more nuclear
159
+ # option (not recommended) you can uncomment the following to ignore the entire idea folder.
160
+ #.idea/
161
+
162
+ .DS_Store
app.py ADDED
@@ -0,0 +1,143 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import openai
2
+ import numpy as np
3
+ from tempfile import NamedTemporaryFile
4
+ import copy
5
+ import shapely
6
+ from shapely.geometry import *
7
+ from shapely.affinity import *
8
+ from omegaconf import OmegaConf
9
+ from moviepy.editor import ImageSequenceClip
10
+ import gradio as gr
11
+
12
+
13
+ from lmp import LMP, LMPFGen
14
+ from sim import PickPlaceEnv, LMP_wrapper
15
+ from consts import ALL_BLOCKS, ALL_BOWLS
16
+
17
+
18
+ class DemoRunner:
19
+
20
+ def __init__(self):
21
+ self._cfg = OmegaConf.to_container(OmegaConf.load('cfg.yaml'), resolve=True)
22
+ self._env = None
23
+ self._model_name = ''
24
+
25
+ def make_LMP(self, env):
26
+ # LMP env wrapper
27
+ cfg = copy.deepcopy(self._cfg)
28
+ cfg['env'] = {
29
+ 'init_objs': list(env.obj_name_to_id.keys()),
30
+ 'coords': cfg['tabletop_coords']
31
+ }
32
+ for vs in cfg['lmps'].values():
33
+ vs['engine'] = self._model_name
34
+
35
+ LMP_env = LMP_wrapper(env, cfg)
36
+ # creating APIs that the LMPs can interact with
37
+ fixed_vars = {
38
+ 'np': np
39
+ }
40
+ fixed_vars.update({
41
+ name: eval(name)
42
+ for name in shapely.geometry.__all__ + shapely.affinity.__all__
43
+ })
44
+ variable_vars = {
45
+ k: getattr(LMP_env, k)
46
+ for k in [
47
+ 'get_bbox', 'get_obj_pos', 'get_color', 'is_obj_visible', 'denormalize_xy',
48
+ 'put_first_on_second', 'get_obj_names',
49
+ 'get_corner_name', 'get_side_name',
50
+ ]
51
+ }
52
+ variable_vars['say'] = lambda msg: print(f'robot says: {msg}')
53
+
54
+ # creating the function-generating LMP
55
+ lmp_fgen = LMPFGen(cfg['lmps']['fgen'], fixed_vars, variable_vars)
56
+
57
+ # creating other low-level LMPs
58
+ variable_vars.update({
59
+ k: LMP(k, cfg['lmps'][k], lmp_fgen, fixed_vars, variable_vars)
60
+ for k in ['parse_obj_name', 'parse_position', 'parse_question', 'transform_shape_pts']
61
+ })
62
+
63
+ # creating the LMP that deals w/ high-level language commands
64
+ lmp_tabletop_ui = LMP(
65
+ 'tabletop_ui', cfg['lmps']['tabletop_ui'], lmp_fgen, fixed_vars, variable_vars
66
+ )
67
+
68
+ return lmp_tabletop_ui
69
+
70
+ def setup(self, api_key, model_name, n_blocks, n_bowls):
71
+ openai.api_key = api_key
72
+ self._model_name = model_name
73
+
74
+ self._env = PickPlaceEnv(render=True, high_res=False, high_frame_rate=False)
75
+ block_list = np.random.choice(ALL_BLOCKS, size=n_blocks, replace=False).tolist()
76
+ bowl_list = np.random.choice(ALL_BOWLS, size=n_bowls, replace=False).tolist()
77
+ obj_list = block_list + bowl_list
78
+ self._env.reset(obj_list)
79
+
80
+ self._lmp_tabletop_ui = self.make_LMP(self._env)
81
+
82
+ info = '## Available objects: \n- ' + '\n- '.join(obj_list)
83
+ img = self._env.get_camera_image()
84
+
85
+ return info, img
86
+
87
+ def run(self, instruction):
88
+ if self._env is None:
89
+ return 'Please run setup first'
90
+
91
+ self._env.cache_video = []
92
+
93
+ self._lmp_tabletop_ui(instruction, f'objects = {self._env.object_list}')
94
+
95
+ video_file_name = ''
96
+ if self._env.cache_video:
97
+ rendered_clip = ImageSequenceClip(self._env.cache_video, fps=25)
98
+ video_file_name = NamedTemporaryFile(suffix='.mp4', delete=False).name
99
+ rendered_clip.write_videofile(video_file_name, fps=25)
100
+
101
+ return 'Done', video_file_name
102
+
103
+
104
+ if __name__ == '__main__':
105
+ demo_runner = DemoRunner()
106
+ demo = gr.Blocks()
107
+
108
+ with demo:
109
+ with gr.Row():
110
+ with gr.Column():
111
+ with gr.Row():
112
+ inp_api_key = gr.Textbox(label='OpenAI API Key', lines=1, value='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')
113
+ inp_model_name = gr.Dropdown(label='Model Name', choices=['code-davinci-002', 'text-davinci-002'], value='code-davinci-002')
114
+ with gr.Row():
115
+ inp_n_blocks = gr.Slider(label='Num Blocks', minimum=0, maximum=3, value=3, step=1)
116
+ inp_n_bowls = gr.Slider(label='Num Bowls', minimum=0, maximum=3, value=3, step=1)
117
+
118
+ btn_setup = gr.Button("1) Setup/Reset Env")
119
+ info_setup = gr.Markdown(label='Setup Info')
120
+ with gr.Column():
121
+ img_setup = gr.Image(label='Setup Image')
122
+
123
+ with gr.Row():
124
+ with gr.Column():
125
+
126
+ inp_instruction = gr.Textbox(label='Instruction', lines=1)
127
+ btn_run = gr.Button("2) Run Instruction")
128
+ info_run = gr.Label(label='Run Info')
129
+ with gr.Column():
130
+ video_run = gr.Video(label='Run Video')
131
+
132
+ btn_setup.click(
133
+ demo_runner.setup,
134
+ inputs=[inp_api_key, inp_model_name, inp_n_blocks, inp_n_bowls],
135
+ outputs=[info_setup, img_setup]
136
+ )
137
+ btn_run.click(
138
+ demo_runner.run,
139
+ inputs=[inp_instruction],
140
+ outputs=[info_run, video_run]
141
+ )
142
+
143
+ demo.launch()
bowl/bowl.urdf ADDED
@@ -0,0 +1,29 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ <?xml version="1.0" ?>
2
+ <robot name="bowl.urdf">
3
+ <link name="baseLink">
4
+ <contact>
5
+ <lateral_friction value="1.0"/>
6
+ <inertia_scaling value="3.0"/>
7
+ </contact>
8
+ <inertial>
9
+ <origin rpy="0 0 0" xyz="-0.025 0 0.02"/>
10
+ <mass value=".1"/>
11
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
12
+ </inertial>
13
+ <visual>
14
+ <origin rpy="0 0 0" xyz="0 0 0"/>
15
+ <geometry>
16
+ <mesh filename="textured-0008192.obj" scale="1.25 1.25 0.25"/>
17
+ </geometry>
18
+ <material name="green">
19
+ <color rgba="0.34901961 0.6627451 0.30980392 1"/>
20
+ </material>
21
+ </visual>
22
+ <collision>
23
+ <origin rpy="0 0 0" xyz="0 0 0"/>
24
+ <geometry>
25
+ <mesh filename="cup.obj" scale="1.25 1.25 0.25"/>
26
+ </geometry>
27
+ </collision>
28
+ </link>
29
+ </robot>
bowl/cup.obj ADDED
@@ -0,0 +1,1413 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Blender v2.69 (sub 0) OBJ File: ''
2
+ # www.blender.org
3
+ mtllib cup_vhacd.mtl
4
+ o ShapeIndexedFaceSet.013_ShapeIndexedFaceSet
5
+ v 0.000000 0.000000 0.000000
6
+ v -0.018137 0.054263 0.137619
7
+ v -0.033504 0.036700 0.027862
8
+ v -0.033504 0.041091 0.027862
9
+ v -0.033504 0.052066 0.139822
10
+ v -0.018137 0.045481 0.122254
11
+ v -0.018137 0.043286 0.027862
12
+ v -0.033504 0.045481 0.139822
13
+ v -0.022528 0.036700 0.030065
14
+ v -0.029114 0.043286 0.027862
15
+ v -0.031308 0.041091 0.100303
16
+ v -0.018137 0.047673 0.139822
17
+ v -0.018137 0.038897 0.027862
18
+ v -0.029114 0.052066 0.122254
19
+ v -0.033504 0.036700 0.045430
20
+ v -0.020333 0.054263 0.135427
21
+ v -0.029114 0.043286 0.122254
22
+ v -0.033504 0.043286 0.041035
23
+ v -0.018137 0.045481 0.045430
24
+ v -0.018137 0.038897 0.041035
25
+ v -0.033504 0.052066 0.135427
26
+ v -0.018137 0.043286 0.098100
27
+ v -0.022528 0.045481 0.045430
28
+ v -0.031308 0.043286 0.032257
29
+ v -0.033504 0.041091 0.100303
30
+ v -0.018137 0.052066 0.139822
31
+ v -0.031308 0.036700 0.045430
32
+ v -0.031308 0.045481 0.139822
33
+ usemtl Shape.027
34
+ s off
35
+ f 17 12 28
36
+ f 4 3 5
37
+ f 3 4 7
38
+ f 2 6 7
39
+ f 5 3 8
40
+ f 7 4 10
41
+ f 6 2 12
42
+ f 5 8 12
43
+ f 3 7 13
44
+ f 7 6 13
45
+ f 9 3 13
46
+ f 8 3 15
47
+ f 3 9 15
48
+ f 5 2 16
49
+ f 10 14 16
50
+ f 11 9 17
51
+ f 6 12 17
52
+ f 4 5 18
53
+ f 2 7 19
54
+ f 7 10 19
55
+ f 16 2 19
56
+ f 13 6 20
57
+ f 9 13 20
58
+ f 5 16 21
59
+ f 16 14 21
60
+ f 18 5 21
61
+ f 6 17 22
62
+ f 17 9 22
63
+ f 20 6 22
64
+ f 9 20 22
65
+ f 10 16 23
66
+ f 19 10 23
67
+ f 16 19 23
68
+ f 10 4 24
69
+ f 14 10 24
70
+ f 4 18 24
71
+ f 21 14 24
72
+ f 18 21 24
73
+ f 8 15 25
74
+ f 15 11 25
75
+ f 17 8 25
76
+ f 11 17 25
77
+ f 2 5 26
78
+ f 12 2 26
79
+ f 5 12 26
80
+ f 9 11 27
81
+ f 15 9 27
82
+ f 11 15 27
83
+ f 12 8 28
84
+ f 8 17 28
85
+ o ShapeIndexedFaceSet.012_ShapeIndexedFaceSet.027
86
+ v 0.000000 0.000000 0.000000
87
+ v -0.033504 0.049872 0.139822
88
+ v -0.046676 0.025724 0.027862
89
+ v -0.046676 0.034505 0.027862
90
+ v -0.033504 0.034505 0.027862
91
+ v -0.046676 0.036704 0.133235
92
+ v -0.046676 0.045480 0.131032
93
+ v -0.035700 0.041091 0.027862
94
+ v -0.033504 0.043284 0.133235
95
+ v -0.042285 0.025724 0.030065
96
+ v -0.037896 0.049872 0.124446
97
+ v -0.044480 0.034505 0.117848
98
+ v -0.033504 0.041091 0.027862
99
+ v -0.044480 0.047676 0.135427
100
+ v -0.046676 0.038892 0.139822
101
+ v -0.042285 0.038892 0.036651
102
+ v -0.033504 0.049872 0.120062
103
+ v -0.035700 0.043284 0.139822
104
+ v -0.040090 0.049872 0.139822
105
+ v -0.046676 0.025724 0.034449
106
+ v -0.037896 0.041091 0.034449
107
+ v -0.033504 0.038892 0.084927
108
+ v -0.046676 0.045480 0.139822
109
+ v -0.044480 0.036704 0.133235
110
+ v -0.044480 0.036704 0.030065
111
+ v -0.033504 0.034505 0.032257
112
+ v -0.042285 0.047676 0.122254
113
+ v -0.046676 0.034505 0.117848
114
+ v -0.035700 0.043284 0.049824
115
+ v -0.042285 0.025724 0.027862
116
+ v -0.044480 0.025724 0.034449
117
+ v -0.033504 0.041091 0.111273
118
+ usemtl Shape.026
119
+ s off
120
+ f 38 50 60
121
+ f 31 32 33
122
+ f 32 31 34
123
+ f 32 34 35
124
+ f 33 32 36
125
+ f 33 30 37
126
+ f 38 37 40
127
+ f 30 33 41
128
+ f 33 36 41
129
+ f 35 34 43
130
+ f 39 30 45
131
+ f 30 41 45
132
+ f 37 30 46
133
+ f 30 43 46
134
+ f 30 39 47
135
+ f 39 42 47
136
+ f 43 30 47
137
+ f 34 31 48
138
+ f 31 38 48
139
+ f 33 37 50
140
+ f 42 35 51
141
+ f 35 43 51
142
+ f 47 42 51
143
+ f 43 47 51
144
+ f 34 40 52
145
+ f 40 37 52
146
+ f 43 34 52
147
+ f 37 46 52
148
+ f 46 43 52
149
+ f 32 35 53
150
+ f 36 32 53
151
+ f 35 42 53
152
+ f 42 44 53
153
+ f 49 36 53
154
+ f 44 49 53
155
+ f 38 33 54
156
+ f 33 50 54
157
+ f 50 38 54
158
+ f 42 39 55
159
+ f 44 42 55
160
+ f 39 49 55
161
+ f 49 44 55
162
+ f 40 34 56
163
+ f 34 48 56
164
+ f 48 40 56
165
+ f 41 36 57
166
+ f 39 45 57
167
+ f 45 41 57
168
+ f 36 49 57
169
+ f 49 39 57
170
+ f 31 33 58
171
+ f 38 31 58
172
+ f 33 38 58
173
+ f 38 40 59
174
+ f 48 38 59
175
+ f 40 48 59
176
+ f 37 38 60
177
+ f 50 37 60
178
+ o ShapeIndexedFaceSet.011_ShapeIndexedFaceSet.026
179
+ v 0.000000 0.000000 0.000000
180
+ v -0.066434 -0.002813 0.135413
181
+ v -0.051068 -0.007203 0.058606
182
+ v -0.051068 -0.009399 0.058606
183
+ v -0.057654 0.005965 0.038839
184
+ v -0.059846 0.005965 0.137608
185
+ v -0.057654 -0.009399 0.139812
186
+ v -0.057654 -0.009399 0.052024
187
+ v -0.053264 0.005965 0.038839
188
+ v -0.066434 0.005965 0.139812
189
+ v -0.064236 -0.009399 0.139812
190
+ v -0.051068 -0.009399 0.038839
191
+ v -0.059846 0.001575 0.049829
192
+ v -0.057654 -0.007203 0.041052
193
+ v -0.055458 -0.007203 0.122248
194
+ v -0.066434 -0.005009 0.137608
195
+ v -0.059846 0.005965 0.049829
196
+ v -0.057654 0.005965 0.109073
197
+ v -0.055458 -0.009399 0.038839
198
+ v -0.053264 0.005965 0.043247
199
+ v -0.066434 0.005965 0.135413
200
+ v -0.051068 -0.005009 0.038839
201
+ v -0.062042 0.005965 0.139812
202
+ v -0.055458 -0.009399 0.122248
203
+ v -0.064236 -0.009399 0.135413
204
+ v -0.057654 -0.007203 0.139812
205
+ v -0.057654 -0.005009 0.038839
206
+ v -0.053264 -0.007203 0.091519
207
+ usemtl Shape.025
208
+ s off
209
+ f 75 84 88
210
+ f 64 67 68
211
+ f 65 66 69
212
+ f 66 65 70
213
+ f 67 70 71
214
+ f 68 67 71
215
+ f 63 64 72
216
+ f 64 68 72
217
+ f 65 69 72
218
+ f 70 62 76
219
+ f 71 70 76
220
+ f 62 73 76
221
+ f 74 68 76
222
+ f 73 74 76
223
+ f 70 65 77
224
+ f 65 73 77
225
+ f 73 62 77
226
+ f 69 66 78
227
+ f 66 75 78
228
+ f 72 68 79
229
+ f 65 72 79
230
+ f 68 74 79
231
+ f 78 63 80
232
+ f 69 78 80
233
+ f 62 70 81
234
+ f 77 62 81
235
+ f 70 77 81
236
+ f 63 72 82
237
+ f 72 69 82
238
+ f 80 63 82
239
+ f 69 80 82
240
+ f 66 70 83
241
+ f 70 67 83
242
+ f 67 64 84
243
+ f 75 67 84
244
+ f 68 71 85
245
+ f 71 76 85
246
+ f 76 68 85
247
+ f 75 66 86
248
+ f 67 75 86
249
+ f 66 83 86
250
+ f 83 67 86
251
+ f 73 65 87
252
+ f 74 73 87
253
+ f 65 79 87
254
+ f 79 74 87
255
+ f 64 63 88
256
+ f 63 78 88
257
+ f 78 75 88
258
+ f 84 64 88
259
+ o ShapeIndexedFaceSet.010_ShapeIndexedFaceSet.025
260
+ v 0.000000 0.000000 0.000000
261
+ v -0.062040 -0.013792 0.122248
262
+ v -0.042286 -0.018180 0.049829
263
+ v -0.053261 -0.009401 0.104685
264
+ v -0.055455 -0.009401 0.038839
265
+ v -0.059844 -0.020377 0.139812
266
+ v -0.051069 -0.020377 0.049829
267
+ v -0.051069 -0.020377 0.139812
268
+ v -0.064236 -0.009401 0.139812
269
+ v -0.048873 -0.009401 0.038839
270
+ v -0.042286 -0.020377 0.038839
271
+ v -0.055455 -0.011597 0.139812
272
+ v -0.053261 -0.015986 0.038839
273
+ v -0.048873 -0.018180 0.122248
274
+ v -0.055455 -0.011597 0.038839
275
+ v -0.057651 -0.009401 0.139812
276
+ v -0.064236 -0.011597 0.137608
277
+ v -0.048873 -0.020377 0.038839
278
+ v -0.042286 -0.020377 0.049829
279
+ v -0.048873 -0.009401 0.045441
280
+ v -0.059844 -0.018180 0.122248
281
+ v -0.055455 -0.013792 0.047635
282
+ v -0.051069 -0.018180 0.139812
283
+ v -0.042286 -0.018180 0.038839
284
+ v -0.053261 -0.011597 0.122248
285
+ v -0.059844 -0.020377 0.137608
286
+ v -0.048873 -0.020377 0.122248
287
+ usemtl Shape.024
288
+ s off
289
+ f 107 102 115
290
+ f 94 95 96
291
+ f 92 93 97
292
+ f 94 96 97
293
+ f 93 92 98
294
+ f 96 95 99
295
+ f 93 98 99
296
+ f 97 96 100
297
+ f 93 99 101
298
+ f 93 101 103
299
+ f 92 97 104
300
+ f 100 92 104
301
+ f 97 100 104
302
+ f 97 93 105
303
+ f 94 97 105
304
+ f 93 103 105
305
+ f 99 95 106
306
+ f 95 101 106
307
+ f 101 99 106
308
+ f 99 91 107
309
+ f 96 99 107
310
+ f 91 102 107
311
+ f 92 91 108
312
+ f 98 92 108
313
+ f 91 98 108
314
+ f 103 101 110
315
+ f 90 105 110
316
+ f 105 103 110
317
+ f 109 90 110
318
+ f 101 109 110
319
+ f 100 96 111
320
+ f 96 102 111
321
+ f 102 100 111
322
+ f 98 91 112
323
+ f 91 99 112
324
+ f 99 98 112
325
+ f 91 92 113
326
+ f 92 100 113
327
+ f 102 91 113
328
+ f 100 102 113
329
+ f 95 94 114
330
+ f 101 95 114
331
+ f 105 90 114
332
+ f 94 105 114
333
+ f 90 109 114
334
+ f 109 101 114
335
+ f 102 96 115
336
+ f 96 107 115
337
+ o ShapeIndexedFaceSet.009_ShapeIndexedFaceSet.024
338
+ v 0.000000 0.000000 0.000000
339
+ v -0.018137 0.043284 0.036653
340
+ v 0.019183 0.034509 0.139822
341
+ v 0.019183 0.030119 0.139822
342
+ v -0.018137 0.047673 0.139822
343
+ v -0.004964 0.052067 0.135419
344
+ v 0.001618 0.038898 0.036653
345
+ v -0.000573 0.030119 0.038855
346
+ v 0.010401 0.030119 0.045439
347
+ v 0.012596 0.030119 0.137610
348
+ v -0.018137 0.038898 0.045439
349
+ v 0.008205 0.045480 0.139822
350
+ v -0.018137 0.052067 0.120048
351
+ v -0.007160 0.043284 0.038855
352
+ v 0.010401 0.032313 0.045439
353
+ v -0.018137 0.043284 0.100295
354
+ v -0.015942 0.045480 0.045439
355
+ v -0.018137 0.052067 0.139822
356
+ v 0.008205 0.045480 0.135419
357
+ v 0.008205 0.030119 0.036653
358
+ v 0.019183 0.030119 0.122250
359
+ v -0.018137 0.038898 0.036653
360
+ v 0.001618 0.047673 0.120048
361
+ v 0.019183 0.034509 0.135419
362
+ v 0.008205 0.030119 0.109071
363
+ v -0.011551 0.052067 0.122250
364
+ v -0.004964 0.052067 0.139822
365
+ v -0.018137 0.045480 0.122250
366
+ v 0.014792 0.038898 0.133217
367
+ v 0.008205 0.034509 0.045439
368
+ v 0.019183 0.032313 0.122250
369
+ v -0.018137 0.045480 0.045439
370
+ v -0.000573 0.041092 0.045439
371
+ v 0.014792 0.030119 0.139822
372
+ v -0.000573 0.030119 0.036653
373
+ v -0.011551 0.045480 0.052024
374
+ v -0.009355 0.043284 0.036653
375
+ v 0.008205 0.032313 0.036653
376
+ v -0.002769 0.049869 0.122250
377
+ v 0.006009 0.045480 0.122250
378
+ usemtl Shape.023
379
+ s off
380
+ f 148 138 155
381
+ f 119 118 120
382
+ f 119 123 124
383
+ f 123 119 125
384
+ f 120 117 126
385
+ f 120 118 127
386
+ f 117 120 128
387
+ f 120 126 131
388
+ f 120 127 133
389
+ f 128 120 133
390
+ f 121 128 133
391
+ f 121 127 134
392
+ f 117 122 135
393
+ f 124 123 135
394
+ f 130 124 135
395
+ f 118 119 136
396
+ f 119 124 136
397
+ f 124 130 136
398
+ f 123 126 137
399
+ f 126 117 137
400
+ f 117 135 137
401
+ f 121 134 138
402
+ f 118 136 139
403
+ f 123 125 140
404
+ f 126 123 140
405
+ f 125 131 140
406
+ f 131 126 140
407
+ f 128 121 141
408
+ f 132 128 141
409
+ f 127 121 142
410
+ f 133 127 142
411
+ f 121 133 142
412
+ f 125 120 143
413
+ f 120 131 143
414
+ f 131 125 143
415
+ f 127 118 144
416
+ f 134 127 144
417
+ f 118 139 144
418
+ f 139 130 144
419
+ f 122 134 145
420
+ f 134 144 145
421
+ f 144 130 145
422
+ f 136 130 146
423
+ f 130 139 146
424
+ f 139 136 146
425
+ f 117 128 147
426
+ f 132 117 147
427
+ f 128 132 147
428
+ f 122 129 148
429
+ f 119 120 149
430
+ f 125 119 149
431
+ f 120 125 149
432
+ f 135 123 150
433
+ f 123 137 150
434
+ f 137 135 150
435
+ f 121 129 151
436
+ f 129 132 151
437
+ f 141 121 151
438
+ f 132 141 151
439
+ f 122 117 152
440
+ f 129 122 152
441
+ f 117 132 152
442
+ f 132 129 152
443
+ f 135 122 153
444
+ f 130 135 153
445
+ f 122 145 153
446
+ f 145 130 153
447
+ f 129 121 154
448
+ f 121 138 154
449
+ f 148 129 154
450
+ f 138 148 154
451
+ f 134 122 155
452
+ f 138 134 155
453
+ f 122 148 155
454
+ o ShapeIndexedFaceSet.008_ShapeIndexedFaceSet.023
455
+ v 0.000000 0.000000 0.000000
456
+ v 0.014792 0.030114 0.139822
457
+ v 0.012598 0.005966 0.036653
458
+ v 0.016986 0.005966 0.036653
459
+ v 0.010404 0.030114 0.036653
460
+ v 0.025768 0.021331 0.139822
461
+ v 0.021376 0.005966 0.139822
462
+ v 0.003816 0.027919 0.049832
463
+ v 0.016986 0.019135 0.036653
464
+ v 0.025768 0.005966 0.120048
465
+ v 0.021376 0.030114 0.135419
466
+ v 0.003816 0.030114 0.036653
467
+ v 0.010404 0.027919 0.113464
468
+ v 0.019180 0.008162 0.043248
469
+ v 0.025768 0.016946 0.122250
470
+ v 0.019180 0.005966 0.122250
471
+ v 0.025768 0.005966 0.139822
472
+ v 0.012598 0.027919 0.036653
473
+ v 0.021376 0.027919 0.120048
474
+ v 0.012598 0.005966 0.041046
475
+ v 0.010404 0.030114 0.113464
476
+ v 0.025768 0.021331 0.135419
477
+ v 0.014792 0.025720 0.137610
478
+ v 0.019180 0.005966 0.043248
479
+ v 0.021376 0.030114 0.139822
480
+ v 0.003816 0.027919 0.036653
481
+ v 0.012598 0.030114 0.049832
482
+ v 0.003816 0.030114 0.049832
483
+ v 0.019180 0.012557 0.047631
484
+ v 0.014792 0.025720 0.045439
485
+ v 0.016986 0.005966 0.095902
486
+ v 0.025768 0.012557 0.120048
487
+ usemtl Shape.022
488
+ s off
489
+ f 169 170 187
490
+ f 159 158 160
491
+ f 158 159 162
492
+ f 161 157 162
493
+ f 159 160 164
494
+ f 162 159 165
495
+ f 160 157 166
496
+ f 160 158 167
497
+ f 157 160 167
498
+ f 159 164 169
499
+ f 161 165 170
500
+ f 158 162 171
501
+ f 168 163 171
502
+ f 162 168 171
503
+ f 161 162 172
504
+ f 165 161 172
505
+ f 162 165 172
506
+ f 164 160 173
507
+ f 163 158 175
508
+ f 158 171 175
509
+ f 157 167 176
510
+ f 163 168 176
511
+ f 168 157 176
512
+ f 166 161 177
513
+ f 161 170 177
514
+ f 170 164 177
515
+ f 174 166 177
516
+ f 164 174 177
517
+ f 162 157 178
518
+ f 157 168 178
519
+ f 168 162 178
520
+ f 165 159 179
521
+ f 159 169 179
522
+ f 169 165 179
523
+ f 157 161 180
524
+ f 166 157 180
525
+ f 161 166 180
526
+ f 158 163 181
527
+ f 167 158 181
528
+ f 163 167 181
529
+ f 160 166 182
530
+ f 173 160 182
531
+ f 166 174 182
532
+ f 174 173 182
533
+ f 167 163 183
534
+ f 176 167 183
535
+ f 163 176 183
536
+ f 169 164 184
537
+ f 170 169 184
538
+ f 164 170 184
539
+ f 164 173 185
540
+ f 173 174 185
541
+ f 174 164 185
542
+ f 171 163 186
543
+ f 163 175 186
544
+ f 175 171 186
545
+ f 165 169 187
546
+ f 170 165 187
547
+ o ShapeIndexedFaceSet.007_ShapeIndexedFaceSet.022
548
+ v 0.000000 0.000000 0.000000
549
+ v -0.051069 0.043286 0.139822
550
+ v -0.057651 0.005966 0.027862
551
+ v -0.057651 0.014748 0.027862
552
+ v -0.066430 0.016948 0.139822
553
+ v -0.059846 0.005966 0.135427
554
+ v -0.046678 0.023530 0.036651
555
+ v -0.046678 0.036700 0.139822
556
+ v -0.048875 0.032305 0.027862
557
+ v -0.051069 0.008162 0.030065
558
+ v -0.062040 0.030110 0.135427
559
+ v -0.066430 0.005966 0.139822
560
+ v -0.046678 0.043286 0.122254
561
+ v -0.046678 0.032305 0.027862
562
+ v -0.055459 0.023530 0.036651
563
+ v -0.057651 0.036700 0.135427
564
+ v -0.064235 0.025722 0.137619
565
+ v -0.053264 0.005966 0.027862
566
+ v -0.066430 0.016948 0.135427
567
+ v -0.046678 0.032305 0.115667
568
+ v -0.059846 0.005966 0.049824
569
+ v -0.051069 0.030110 0.030065
570
+ v -0.057651 0.005966 0.109081
571
+ v -0.051069 0.043286 0.135427
572
+ v -0.062040 0.005966 0.139822
573
+ v -0.046678 0.043286 0.139822
574
+ v -0.046678 0.023530 0.027862
575
+ v -0.062040 0.030110 0.139822
576
+ v -0.048875 0.043286 0.122254
577
+ v -0.066430 0.005966 0.135427
578
+ v -0.055459 0.021335 0.027862
579
+ v -0.053264 0.005966 0.043238
580
+ v -0.064235 0.021335 0.122254
581
+ v -0.057651 0.016948 0.034449
582
+ v -0.059846 0.008162 0.049824
583
+ usemtl Shape.021
584
+ s off
585
+ f 208 206 222
586
+ f 189 192 195
587
+ f 190 191 196
588
+ f 190 193 199
589
+ f 195 192 199
590
+ f 195 194 200
591
+ f 196 200 201
592
+ f 190 196 201
593
+ f 200 194 201
594
+ f 198 202 204
595
+ f 193 190 205
596
+ f 190 201 205
597
+ f 199 192 206
598
+ f 192 204 206
599
+ f 194 195 207
600
+ f 197 194 207
601
+ f 191 190 208
602
+ f 190 199 208
603
+ f 202 198 209
604
+ f 203 196 209
605
+ f 198 203 209
606
+ f 195 193 210
607
+ f 193 205 210
608
+ f 207 195 210
609
+ f 197 207 210
610
+ f 189 200 211
611
+ f 203 189 211
612
+ f 196 203 211
613
+ f 193 195 212
614
+ f 199 193 212
615
+ f 195 199 212
616
+ f 189 195 213
617
+ f 200 189 213
618
+ f 195 200 213
619
+ f 194 197 214
620
+ f 201 194 214
621
+ f 197 205 214
622
+ f 205 201 214
623
+ f 192 189 215
624
+ f 189 203 215
625
+ f 203 198 215
626
+ f 198 204 215
627
+ f 204 192 215
628
+ f 200 196 216
629
+ f 211 200 216
630
+ f 196 211 216
631
+ f 199 206 217
632
+ f 208 199 217
633
+ f 206 208 217
634
+ f 196 191 218
635
+ f 204 202 218
636
+ f 209 196 218
637
+ f 202 209 218
638
+ f 205 197 219
639
+ f 210 205 219
640
+ f 197 210 219
641
+ f 191 206 220
642
+ f 206 204 220
643
+ f 218 191 221
644
+ f 204 218 221
645
+ f 191 220 221
646
+ f 220 204 221
647
+ f 206 191 222
648
+ f 191 208 222
649
+ o ShapeIndexedFaceSet.006_ShapeIndexedFaceSet.021
650
+ v 0.000000 0.000000 0.000000
651
+ v 0.023572 -0.013791 0.139822
652
+ v 0.001623 -0.020377 0.041034
653
+ v 0.008210 -0.020377 0.041034
654
+ v 0.014791 0.005966 0.041034
655
+ v 0.021376 0.005966 0.139822
656
+ v 0.012600 -0.020377 0.139822
657
+ v 0.019181 0.001575 0.043239
658
+ v 0.025768 0.005966 0.120056
659
+ v 0.001623 -0.018180 0.045434
660
+ v 0.019181 -0.020377 0.124437
661
+ v 0.014791 -0.013791 0.045434
662
+ v 0.008210 -0.018180 0.113471
663
+ v 0.016987 0.005966 0.098096
664
+ v 0.025768 -0.007205 0.135422
665
+ v 0.025768 0.005966 0.139822
666
+ v 0.016987 -0.007205 0.041034
667
+ v 0.019181 0.005966 0.043239
668
+ v 0.014791 0.005966 0.067384
669
+ v 0.023572 -0.011594 0.122251
670
+ v 0.025768 -0.002819 0.122251
671
+ v 0.010404 -0.020377 0.045434
672
+ v 0.019181 -0.020377 0.139822
673
+ v 0.019181 0.005966 0.122251
674
+ v 0.012600 -0.018180 0.139822
675
+ v 0.008210 -0.020377 0.113471
676
+ v 0.025768 -0.005014 0.126642
677
+ v 0.012600 -0.018180 0.049824
678
+ v 0.012600 -0.015986 0.041034
679
+ v 0.001623 -0.018180 0.041034
680
+ v 0.025768 -0.007205 0.139822
681
+ v 0.021376 -0.018180 0.135422
682
+ v 0.006014 -0.018180 0.091520
683
+ v 0.019181 -0.002819 0.052019
684
+ v 0.016987 -0.009400 0.047629
685
+ v 0.025768 -0.000620 0.120056
686
+ v 0.021376 -0.015986 0.122251
687
+ usemtl Shape.020
688
+ s off
689
+ f 254 233 259
690
+ f 226 225 227
691
+ f 225 226 229
692
+ f 224 228 229
693
+ f 227 228 231
694
+ f 229 226 233
695
+ f 228 227 236
696
+ f 228 224 238
697
+ f 231 228 238
698
+ f 237 231 238
699
+ f 226 227 239
700
+ f 227 231 240
701
+ f 231 230 240
702
+ f 239 227 240
703
+ f 230 239 240
704
+ f 227 232 241
705
+ f 232 236 241
706
+ f 236 227 241
707
+ f 237 224 242
708
+ f 231 237 243
709
+ f 233 226 244
710
+ f 224 229 245
711
+ f 229 233 245
712
+ f 235 228 246
713
+ f 236 232 246
714
+ f 228 236 246
715
+ f 229 228 247
716
+ f 235 229 247
717
+ f 228 235 247
718
+ f 225 229 248
719
+ f 232 225 248
720
+ f 229 235 248
721
+ f 237 242 249
722
+ f 242 239 249
723
+ f 243 237 249
724
+ f 234 233 250
725
+ f 233 244 250
726
+ f 250 244 251
727
+ f 226 239 251
728
+ f 239 234 251
729
+ f 244 226 251
730
+ f 234 250 251
731
+ f 227 225 252
732
+ f 225 232 252
733
+ f 232 227 252
734
+ f 224 237 253
735
+ f 238 224 253
736
+ f 237 238 253
737
+ f 242 224 254
738
+ f 224 245 254
739
+ f 245 233 254
740
+ f 235 246 255
741
+ f 246 232 255
742
+ f 232 248 255
743
+ f 248 235 255
744
+ f 239 230 256
745
+ f 230 243 256
746
+ f 249 239 256
747
+ f 243 249 256
748
+ f 234 239 257
749
+ f 242 234 257
750
+ f 239 242 257
751
+ f 230 231 258
752
+ f 243 230 258
753
+ f 231 243 258
754
+ f 233 234 259
755
+ f 234 242 259
756
+ f 242 254 259
757
+ o ShapeIndexedFaceSet.005_ShapeIndexedFaceSet.020
758
+ v 0.000000 0.000000 0.000000
759
+ v -0.004961 -0.037936 0.122251
760
+ v 0.019183 -0.020377 0.139822
761
+ v 0.019183 -0.022576 0.139822
762
+ v 0.006008 -0.020377 0.041034
763
+ v -0.020333 -0.033546 0.133227
764
+ v -0.020333 -0.033546 0.045434
765
+ v -0.018136 -0.026966 0.043239
766
+ v -0.000575 -0.029160 0.045434
767
+ v 0.010398 -0.020377 0.139822
768
+ v -0.020333 -0.040133 0.139822
769
+ v 0.003815 -0.035741 0.139822
770
+ v -0.000575 -0.020377 0.052019
771
+ v 0.008205 -0.022576 0.045434
772
+ v 0.010398 -0.031351 0.135422
773
+ v -0.007154 -0.031351 0.041034
774
+ v -0.009351 -0.040133 0.135422
775
+ v -0.020333 -0.029160 0.071784
776
+ v -0.020333 -0.029160 0.041034
777
+ v -0.020333 -0.035741 0.139822
778
+ v -0.020333 -0.040133 0.128837
779
+ v -0.015939 -0.033546 0.045434
780
+ v -0.000575 -0.037936 0.137607
781
+ v 0.006008 -0.020377 0.111276
782
+ v -0.000575 -0.020377 0.041034
783
+ v 0.008205 -0.020377 0.045434
784
+ v 0.019183 -0.022576 0.137607
785
+ v -0.020333 -0.031351 0.104681
786
+ v 0.003815 -0.026966 0.052019
787
+ v 0.001618 -0.026966 0.041034
788
+ v 0.003815 -0.035741 0.137607
789
+ v -0.009351 -0.040133 0.139822
790
+ v 0.016985 -0.024771 0.135422
791
+ v -0.020333 -0.031351 0.041034
792
+ v -0.011545 -0.033546 0.054224
793
+ v -0.018136 -0.040133 0.128837
794
+ v 0.010398 -0.031351 0.139822
795
+ usemtl Shape.019
796
+ s off
797
+ f 274 292 296
798
+ f 263 262 269
799
+ f 262 264 269
800
+ f 265 266 270
801
+ f 263 269 270
802
+ f 263 270 271
803
+ f 269 264 272
804
+ f 275 261 276
805
+ f 266 265 277
806
+ f 264 275 278
807
+ f 266 277 278
808
+ f 277 267 278
809
+ f 269 265 279
810
+ f 265 270 279
811
+ f 270 269 279
812
+ f 270 266 280
813
+ f 276 270 280
814
+ f 266 275 281
815
+ f 280 266 281
816
+ f 261 275 282
817
+ f 275 268 282
818
+ f 276 261 282
819
+ f 272 267 283
820
+ f 269 272 283
821
+ f 267 277 283
822
+ f 272 264 284
823
+ f 267 272 284
824
+ f 264 278 284
825
+ f 278 267 284
826
+ f 264 262 285
827
+ f 273 264 285
828
+ f 273 285 286
829
+ f 262 263 286
830
+ f 285 262 286
831
+ f 265 269 287
832
+ f 277 265 287
833
+ f 269 283 287
834
+ f 283 277 287
835
+ f 274 268 288
836
+ f 273 274 288
837
+ f 264 273 289
838
+ f 275 264 289
839
+ f 268 275 289
840
+ f 288 268 289
841
+ f 273 288 289
842
+ f 268 274 290
843
+ f 274 271 290
844
+ f 282 268 290
845
+ f 271 282 290
846
+ f 271 270 291
847
+ f 270 276 291
848
+ f 282 271 291
849
+ f 276 282 291
850
+ f 274 273 292
851
+ f 273 286 292
852
+ f 286 263 292
853
+ f 275 266 293
854
+ f 266 278 293
855
+ f 278 275 293
856
+ f 275 276 294
857
+ f 276 281 294
858
+ f 281 275 294
859
+ f 276 280 295
860
+ f 281 276 295
861
+ f 280 281 295
862
+ f 263 271 296
863
+ f 271 274 296
864
+ f 292 263 296
865
+ o ShapeIndexedFaceSet.004_ShapeIndexedFaceSet.019
866
+ v 0.000000 0.000000 0.000000
867
+ v -0.046671 -0.022576 0.038839
868
+ v -0.020333 -0.035741 0.139812
869
+ v -0.020333 -0.040133 0.139812
870
+ v -0.057652 -0.020377 0.139812
871
+ v -0.022528 -0.026966 0.038839
872
+ v -0.037897 -0.037936 0.135413
873
+ v -0.024724 -0.033546 0.045441
874
+ v -0.048870 -0.020377 0.139812
875
+ v -0.040088 -0.020377 0.038839
876
+ v -0.046671 -0.033546 0.139812
877
+ v -0.033509 -0.031351 0.038839
878
+ v -0.020333 -0.031351 0.106879
879
+ v -0.020333 -0.033546 0.045441
880
+ v -0.048870 -0.022576 0.045441
881
+ v -0.031314 -0.040133 0.137608
882
+ v -0.057652 -0.022576 0.135413
883
+ v -0.040088 -0.029160 0.045441
884
+ v -0.048870 -0.020377 0.045441
885
+ v -0.020333 -0.029160 0.071781
886
+ v -0.040088 -0.020377 0.054218
887
+ v -0.053261 -0.026966 0.126637
888
+ v -0.020333 -0.033546 0.131025
889
+ v -0.020333 -0.040133 0.131025
890
+ v -0.020333 -0.031351 0.038839
891
+ v -0.046671 -0.033546 0.137608
892
+ v -0.046671 -0.024771 0.047635
893
+ v -0.022528 -0.026966 0.043247
894
+ v -0.046671 -0.020377 0.122248
895
+ v -0.042284 -0.026966 0.038839
896
+ v -0.031314 -0.040133 0.139812
897
+ v -0.020333 -0.029160 0.038839
898
+ v -0.044476 -0.033546 0.122248
899
+ v -0.022528 -0.033546 0.137608
900
+ v -0.057652 -0.022576 0.139812
901
+ v -0.046671 -0.020377 0.038839
902
+ v -0.022528 -0.040133 0.131025
903
+ v -0.035701 -0.031351 0.047635
904
+ v -0.057652 -0.020377 0.135413
905
+ usemtl Shape.018
906
+ s off
907
+ f 315 313 335
908
+ f 300 299 301
909
+ f 301 299 305
910
+ f 302 298 306
911
+ f 301 305 306
912
+ f 300 301 307
913
+ f 298 302 308
914
+ f 299 300 309
915
+ f 304 308 310
916
+ f 309 300 310
917
+ f 303 308 312
918
+ f 308 304 312
919
+ f 301 306 315
920
+ f 298 311 315
921
+ f 311 313 315
922
+ f 305 309 316
923
+ f 309 310 316
924
+ f 302 306 317
925
+ f 306 305 317
926
+ f 313 311 318
927
+ f 299 309 319
928
+ f 309 305 319
929
+ f 310 300 320
930
+ f 304 310 320
931
+ f 300 312 320
932
+ f 308 302 321
933
+ f 310 308 321
934
+ f 316 310 321
935
+ f 303 307 322
936
+ f 307 318 322
937
+ f 322 318 323
938
+ f 318 311 323
939
+ f 314 322 323
940
+ f 316 302 324
941
+ f 302 317 324
942
+ f 324 317 325
943
+ f 305 316 325
944
+ f 317 305 325
945
+ f 316 324 325
946
+ f 298 308 326
947
+ f 311 298 326
948
+ f 308 314 326
949
+ f 323 311 326
950
+ f 314 323 326
951
+ f 300 307 327
952
+ f 307 303 327
953
+ f 312 300 327
954
+ f 303 312 327
955
+ f 302 316 328
956
+ f 321 302 328
957
+ f 316 321 328
958
+ f 314 303 329
959
+ f 303 322 329
960
+ f 322 314 329
961
+ f 305 299 330
962
+ f 299 319 330
963
+ f 319 305 330
964
+ f 307 301 331
965
+ f 301 313 331
966
+ f 318 307 331
967
+ f 313 318 331
968
+ f 306 298 332
969
+ f 298 315 332
970
+ f 315 306 332
971
+ f 312 304 333
972
+ f 304 320 333
973
+ f 320 312 333
974
+ f 308 303 334
975
+ f 303 314 334
976
+ f 314 308 334
977
+ f 313 301 335
978
+ f 301 315 335
979
+ o ShapeIndexedFaceSet.003_ShapeIndexedFaceSet.018
980
+ v 0.000000 0.000000 0.000000
981
+ v 0.001618 0.038895 0.036643
982
+ v -0.004962 0.005966 -0.000677
983
+ v 0.014790 0.005966 -0.000677
984
+ v -0.013743 0.041091 -0.000677
985
+ v -0.018137 0.005966 0.025666
986
+ v 0.016987 0.005966 0.036643
987
+ v -0.018137 0.043286 0.036643
988
+ v 0.008206 0.030110 0.003714
989
+ v -0.018137 0.005966 0.001519
990
+ v 0.014790 0.023530 0.036643
991
+ v -0.018137 0.041091 -0.000677
992
+ v 0.014790 0.016948 0.001519
993
+ v -0.004962 0.038895 -0.000677
994
+ v 0.010400 0.005966 0.036643
995
+ v -0.009353 0.043286 0.030057
996
+ v -0.018137 0.038895 0.036643
997
+ v 0.016987 0.014748 0.023467
998
+ v 0.012597 0.021335 -0.000677
999
+ v 0.012597 0.027918 0.034448
1000
+ v 0.016987 0.005966 0.012500
1001
+ v 0.006009 0.032305 0.001519
1002
+ v -0.018137 0.023530 -0.000677
1003
+ v -0.018137 0.043286 0.019079
1004
+ v -0.002765 0.038895 0.005910
1005
+ v 0.016987 0.016948 0.036643
1006
+ v 0.008206 0.032305 0.023467
1007
+ v 0.012597 0.023530 0.005910
1008
+ v -0.009353 0.043286 0.036643
1009
+ v -0.009353 0.041091 0.003714
1010
+ v -0.002765 0.041091 0.032252
1011
+ v 0.014790 0.023530 0.032252
1012
+ v -0.013743 0.043286 0.019079
1013
+ v 0.003816 0.036700 0.027861
1014
+ v 0.001618 0.036700 0.010301
1015
+ v 0.016987 0.016948 0.030057
1016
+ v 0.008206 0.027918 -0.000677
1017
+ usemtl Shape.017
1018
+ s off
1019
+ f 357 344 372
1020
+ f 339 338 340
1021
+ f 338 339 341
1022
+ f 341 339 342
1023
+ f 342 337 343
1024
+ f 338 341 345
1025
+ f 341 343 345
1026
+ f 337 342 346
1027
+ f 340 338 347
1028
+ f 345 343 347
1029
+ f 339 340 349
1030
+ f 341 342 350
1031
+ f 342 343 350
1032
+ f 343 341 352
1033
+ f 341 350 352
1034
+ f 350 343 352
1035
+ f 348 339 354
1036
+ f 339 349 354
1037
+ f 337 346 355
1038
+ f 342 339 356
1039
+ f 339 348 356
1040
+ f 353 342 356
1041
+ f 348 353 356
1042
+ f 338 345 358
1043
+ f 347 338 358
1044
+ f 345 347 358
1045
+ f 340 347 359
1046
+ f 347 343 359
1047
+ f 343 351 359
1048
+ f 357 349 360
1049
+ f 346 342 361
1050
+ f 342 353 361
1051
+ f 355 344 362
1052
+ f 344 357 362
1053
+ f 354 344 363
1054
+ f 348 354 363
1055
+ f 344 355 363
1056
+ f 343 337 364
1057
+ f 351 343 364
1058
+ f 349 340 365
1059
+ f 351 360 365
1060
+ f 360 349 365
1061
+ f 337 360 366
1062
+ f 360 351 366
1063
+ f 364 337 366
1064
+ f 351 364 366
1065
+ f 355 346 367
1066
+ f 346 361 367
1067
+ f 348 363 367
1068
+ f 363 355 367
1069
+ f 340 359 368
1070
+ f 359 351 368
1071
+ f 365 340 368
1072
+ f 351 365 368
1073
+ f 337 355 369
1074
+ f 355 362 369
1075
+ f 362 357 369
1076
+ f 360 337 370
1077
+ f 357 360 370
1078
+ f 337 369 370
1079
+ f 369 357 370
1080
+ f 353 348 371
1081
+ f 361 353 371
1082
+ f 348 367 371
1083
+ f 367 361 371
1084
+ f 344 354 372
1085
+ f 354 349 372
1086
+ f 349 357 372
1087
+ o ShapeIndexedFaceSet.002_ShapeIndexedFaceSet.017
1088
+ v 0.000000 0.000000 0.000000
1089
+ v -0.053255 0.021335 0.001519
1090
+ v -0.018137 0.043286 0.027862
1091
+ v -0.018137 0.043286 0.016883
1092
+ v -0.018137 0.005966 0.025666
1093
+ v -0.057649 0.005966 0.027862
1094
+ v -0.018137 0.005966 0.001519
1095
+ v -0.046672 0.034504 0.027862
1096
+ v -0.026925 0.041091 -0.000677
1097
+ v -0.055452 0.005966 -0.000677
1098
+ v -0.018137 0.041091 -0.000677
1099
+ v -0.042283 0.034504 -0.000677
1100
+ v -0.035704 0.041091 0.023468
1101
+ v -0.057649 0.014748 0.027862
1102
+ v -0.018137 0.025722 -0.000677
1103
+ v -0.053255 0.025722 0.021272
1104
+ v -0.029117 0.043286 0.027862
1105
+ v -0.048865 0.027918 -0.000677
1106
+ v -0.040086 0.005966 -0.000677
1107
+ v -0.037893 0.038895 0.008106
1108
+ v -0.055452 0.016948 0.003718
1109
+ v -0.018137 0.036700 0.027862
1110
+ v -0.051062 0.005966 0.027862
1111
+ v -0.044476 0.034504 0.005911
1112
+ v -0.057649 0.008162 0.016883
1113
+ v -0.031314 0.041091 0.003718
1114
+ v -0.051062 0.027918 0.012500
1115
+ v -0.055452 0.021335 0.027862
1116
+ v -0.055452 0.012553 -0.000677
1117
+ v -0.040086 0.038895 0.021272
1118
+ v -0.026925 0.043286 0.021272
1119
+ v -0.040086 0.038895 0.027862
1120
+ v -0.053255 0.023530 0.008106
1121
+ v -0.055452 0.021335 0.023468
1122
+ v -0.035704 0.038895 0.001519
1123
+ usemtl Shape.016
1124
+ s off
1125
+ f 392 398 407
1126
+ f 376 375 377
1127
+ f 376 377 379
1128
+ f 377 378 379
1129
+ f 378 375 380
1130
+ f 379 378 382
1131
+ f 382 381 383
1132
+ f 376 379 383
1133
+ f 381 376 383
1134
+ f 381 382 384
1135
+ f 378 380 386
1136
+ f 382 383 387
1137
+ f 383 379 387
1138
+ f 375 376 389
1139
+ f 380 375 389
1140
+ f 384 382 390
1141
+ f 379 382 391
1142
+ f 387 379 391
1143
+ f 382 387 391
1144
+ f 377 375 394
1145
+ f 375 378 394
1146
+ f 394 378 395
1147
+ f 378 377 395
1148
+ f 377 394 395
1149
+ f 384 390 396
1150
+ f 382 378 397
1151
+ f 378 386 397
1152
+ f 386 393 397
1153
+ f 385 389 398
1154
+ f 392 385 398
1155
+ f 388 380 399
1156
+ f 380 396 399
1157
+ f 396 390 399
1158
+ f 386 380 400
1159
+ f 380 388 400
1160
+ f 374 390 401
1161
+ f 390 382 401
1162
+ f 393 374 401
1163
+ f 382 397 401
1164
+ f 397 393 401
1165
+ f 385 392 402
1166
+ f 396 380 402
1167
+ f 392 396 402
1168
+ f 376 381 403
1169
+ f 389 376 403
1170
+ f 381 398 403
1171
+ f 398 389 403
1172
+ f 380 389 404
1173
+ f 389 385 404
1174
+ f 402 380 404
1175
+ f 385 402 404
1176
+ f 390 374 405
1177
+ f 374 393 405
1178
+ f 388 399 405
1179
+ f 399 390 405
1180
+ f 393 386 406
1181
+ f 386 400 406
1182
+ f 400 388 406
1183
+ f 405 393 406
1184
+ f 388 405 406
1185
+ f 381 384 407
1186
+ f 384 396 407
1187
+ f 396 392 407
1188
+ f 398 381 407
1189
+ o ShapeIndexedFaceSet.001_ShapeIndexedFaceSet.016
1190
+ v 0.000000 0.000000 0.000000
1191
+ v 0.006010 -0.022571 0.023469
1192
+ v 0.016987 0.005966 0.041034
1193
+ v 0.012596 0.005966 0.041034
1194
+ v -0.004964 0.005966 -0.000677
1195
+ v -0.020333 -0.031353 0.041034
1196
+ v -0.020333 0.005966 0.025666
1197
+ v -0.020333 -0.029158 -0.000677
1198
+ v 0.014792 -0.002819 -0.000677
1199
+ v 0.012596 -0.015985 0.041034
1200
+ v -0.020333 0.005966 0.001520
1201
+ v -0.002765 -0.026962 0.001520
1202
+ v -0.007160 -0.031353 0.041034
1203
+ v 0.014792 0.005966 -0.000677
1204
+ v 0.010401 -0.013789 -0.000677
1205
+ v 0.016987 -0.007210 0.036641
1206
+ v -0.020333 -0.026962 0.041034
1207
+ v -0.015942 -0.031353 0.012503
1208
+ v 0.016987 0.005966 0.012503
1209
+ v 0.001619 -0.026962 0.041034
1210
+ v 0.010401 -0.015985 0.005913
1211
+ v -0.011551 -0.029158 -0.000677
1212
+ v 0.003814 -0.022571 0.001520
1213
+ v -0.020333 -0.013789 -0.000677
1214
+ v 0.014792 -0.007210 0.008106
1215
+ v -0.007160 -0.029158 0.005913
1216
+ v 0.012596 -0.015985 0.032247
1217
+ v 0.001619 -0.026962 0.034444
1218
+ v 0.016987 -0.000620 0.016888
1219
+ v -0.020333 -0.031353 0.012503
1220
+ v 0.016987 -0.007210 0.041034
1221
+ v -0.002765 -0.029158 0.030055
1222
+ v -0.000577 -0.024767 -0.000677
1223
+ v 0.012596 -0.011601 0.003717
1224
+ v -0.007160 -0.031353 0.036641
1225
+ v 0.008205 -0.020376 0.021273
1226
+ v 0.014792 -0.011601 0.030055
1227
+ v 0.008205 -0.018180 0.001520
1228
+ v 0.001619 -0.024767 0.005913
1229
+ usemtl Shape.015
1230
+ s off
1231
+ f 409 435 446
1232
+ f 411 410 412
1233
+ f 410 411 413
1234
+ f 411 412 414
1235
+ f 413 414 415
1236
+ f 415 412 416
1237
+ f 410 413 417
1238
+ f 414 412 418
1239
+ f 415 414 418
1240
+ f 417 413 420
1241
+ f 412 410 421
1242
+ f 416 412 421
1243
+ f 415 416 422
1244
+ f 413 411 424
1245
+ f 411 414 424
1246
+ f 414 413 424
1247
+ f 420 413 425
1248
+ f 421 410 426
1249
+ f 416 421 426
1250
+ f 410 423 426
1251
+ f 417 420 427
1252
+ f 415 422 429
1253
+ f 425 415 429
1254
+ f 412 415 431
1255
+ f 418 412 431
1256
+ f 415 418 431
1257
+ f 429 419 433
1258
+ f 425 429 433
1259
+ f 417 427 434
1260
+ f 434 427 435
1261
+ f 416 426 436
1262
+ f 426 423 436
1263
+ f 432 416 436
1264
+ f 423 432 436
1265
+ f 413 415 437
1266
+ f 425 413 437
1267
+ f 415 425 437
1268
+ f 410 417 438
1269
+ f 423 410 438
1270
+ f 427 420 439
1271
+ f 433 419 439
1272
+ f 419 435 439
1273
+ f 435 427 439
1274
+ f 419 429 440
1275
+ f 429 422 440
1276
+ f 430 419 440
1277
+ f 422 430 440
1278
+ f 422 416 441
1279
+ f 416 432 441
1280
+ f 434 428 441
1281
+ f 420 425 442
1282
+ f 425 433 442
1283
+ f 439 420 442
1284
+ f 433 439 442
1285
+ f 409 430 443
1286
+ f 428 434 443
1287
+ f 435 409 443
1288
+ f 434 435 443
1289
+ f 432 423 444
1290
+ f 417 434 444
1291
+ f 438 417 444
1292
+ f 423 438 444
1293
+ f 441 432 444
1294
+ f 434 441 444
1295
+ f 430 422 445
1296
+ f 422 441 445
1297
+ f 441 428 445
1298
+ f 443 430 445
1299
+ f 428 443 445
1300
+ f 430 409 446
1301
+ f 419 430 446
1302
+ f 435 419 446
1303
+ o ShapeIndexedFaceSet_ShapeIndexedFaceSet.015
1304
+ v 0.000000 0.000000 0.000000
1305
+ v -0.057652 -0.000620 0.021270
1306
+ v -0.022528 -0.031353 0.012498
1307
+ v -0.020333 -0.031353 0.012498
1308
+ v -0.020333 -0.031353 0.038838
1309
+ v -0.020333 0.005966 0.001520
1310
+ v -0.051066 -0.013789 -0.000677
1311
+ v -0.053261 0.005966 0.038838
1312
+ v -0.042284 -0.026962 0.038838
1313
+ v -0.020333 0.005966 0.025664
1314
+ v -0.055457 0.005966 -0.000677
1315
+ v -0.026919 -0.029158 -0.000677
1316
+ v -0.055457 -0.011601 0.038838
1317
+ v -0.037897 -0.026962 0.001520
1318
+ v -0.020333 -0.029158 -0.000677
1319
+ v -0.057652 0.005966 0.038838
1320
+ v -0.031314 -0.031353 0.030050
1321
+ v -0.020333 -0.026962 0.038838
1322
+ v -0.048870 -0.018180 0.001520
1323
+ v -0.055457 -0.005015 0.001520
1324
+ v -0.020333 -0.013789 -0.000677
1325
+ v -0.053261 -0.015985 0.036641
1326
+ v -0.040088 0.005966 -0.000677
1327
+ v -0.046671 -0.022571 0.023467
1328
+ v -0.057652 -0.005015 0.038838
1329
+ v -0.031314 -0.031353 0.038838
1330
+ v -0.033509 -0.029158 0.005911
1331
+ v -0.057652 0.005966 0.016892
1332
+ v -0.053261 -0.011601 0.005911
1333
+ v -0.037897 -0.029158 0.032247
1334
+ v -0.044476 -0.022571 0.001520
1335
+ v -0.040088 -0.024767 -0.000677
1336
+ v -0.057652 -0.005015 0.032247
1337
+ v -0.042284 -0.026962 0.034444
1338
+ v -0.055457 -0.011601 0.034444
1339
+ v -0.055457 -0.000620 -0.000677
1340
+ v -0.026919 -0.031353 0.016892
1341
+ v -0.031314 -0.029158 0.001520
1342
+ usemtl Shape.014
1343
+ s off
1344
+ f 449 483 484
1345
+ f 449 450 451
1346
+ f 451 450 452
1347
+ f 451 454 455
1348
+ f 451 452 456
1349
+ f 452 454 456
1350
+ f 454 452 457
1351
+ f 450 449 458
1352
+ f 453 457 458
1353
+ f 455 454 459
1354
+ f 452 450 461
1355
+ f 450 458 461
1356
+ f 458 457 461
1357
+ f 454 457 462
1358
+ f 459 454 462
1359
+ f 449 451 463
1360
+ f 454 451 464
1361
+ f 451 456 464
1362
+ f 456 454 464
1363
+ f 452 461 467
1364
+ f 461 457 467
1365
+ f 455 459 468
1366
+ f 457 452 469
1367
+ f 452 467 469
1368
+ f 467 457 469
1369
+ f 468 465 470
1370
+ f 462 448 471
1371
+ f 459 462 471
1372
+ f 451 455 472
1373
+ f 463 451 472
1374
+ f 448 462 474
1375
+ f 462 457 474
1376
+ f 466 448 474
1377
+ f 453 465 475
1378
+ f 466 453 475
1379
+ f 465 468 475
1380
+ f 472 455 476
1381
+ f 463 472 476
1382
+ f 460 473 476
1383
+ f 473 463 476
1384
+ f 470 465 477
1385
+ f 453 458 478
1386
+ f 465 453 478
1387
+ f 460 477 478
1388
+ f 477 465 478
1389
+ f 448 466 479
1390
+ f 471 448 479
1391
+ f 459 471 479
1392
+ f 455 468 480
1393
+ f 468 470 480
1394
+ f 476 455 480
1395
+ f 460 476 480
1396
+ f 477 460 480
1397
+ f 470 477 480
1398
+ f 468 459 481
1399
+ f 466 475 481
1400
+ f 475 468 481
1401
+ f 479 466 481
1402
+ f 459 479 481
1403
+ f 457 453 482
1404
+ f 453 466 482
1405
+ f 474 457 482
1406
+ f 466 474 482
1407
+ f 449 463 483
1408
+ f 463 473 483
1409
+ f 483 473 484
1410
+ f 458 449 484
1411
+ f 473 460 484
1412
+ f 478 458 484
1413
+ f 460 478 484
bowl/textured-0008192.obj ADDED
The diff for this file is too large to render. See raw diff
 
cfg.yaml ADDED
@@ -0,0 +1,90 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+
2
+ lmps:
3
+ tabletop_ui:
4
+ prompt_path: prompts/tabletop_ui.py
5
+ engine: model_name
6
+ max_tokens: 512
7
+ temperature: 0
8
+ query_prefix: '# '
9
+ query_suffix: '.'
10
+ stop: ['#', 'objects = [']
11
+ maintain_session: True
12
+ debug_mode: False
13
+ include_context: True
14
+ has_return: False
15
+ return_val_name: ret_val
16
+ parse_obj_name:
17
+ prompt_path: prompts/parse_obj_name.py
18
+ engine: model_name
19
+ max_tokens: 512
20
+ temperature: 0
21
+ query_prefix: '# '
22
+ query_suffix: '.'
23
+ stop: ['#', 'objects = [']
24
+ maintain_session: False
25
+ debug_mode: False
26
+ include_context: True
27
+ has_return: True
28
+ return_val_name: ret_val
29
+ parse_position:
30
+ prompt_path: prompts/parse_position.py
31
+ engine: model_name
32
+ max_tokens: 512
33
+ temperature: 0
34
+ query_prefix: '# '
35
+ query_suffix: '.'
36
+ stop: ['#']
37
+ maintain_session: False
38
+ debug_mode: False
39
+ include_context: True
40
+ has_return: True
41
+ return_val_name: ret_val
42
+ parse_question:
43
+ prompt_path: prompts/parse_question.py
44
+ engine: model_name
45
+ max_tokens: 512
46
+ temperature: 0
47
+ query_prefix: '# '
48
+ query_suffix: '.'
49
+ stop: ['#', 'objects = [']
50
+ maintain_session: False
51
+ debug_mode: False
52
+ include_context: True
53
+ has_return: True
54
+ return_val_name: ret_val
55
+ transform_shape_pts:
56
+ prompt_path: prompts/transform_shape_pts.py
57
+ engine: model_name
58
+ max_tokens: 512
59
+ temperature: 0
60
+ query_prefix: '# '
61
+ query_suffix: '.'
62
+ stop: ['#']
63
+ maintain_session: False
64
+ debug_mode: False
65
+ include_context: True
66
+ has_return: True
67
+ return_val_name: new_shape_pts
68
+ fgen:
69
+ prompt_path: prompts/fgen.py
70
+ engine: model_name
71
+ max_tokens: 512
72
+ temperature: 0
73
+ query_prefix: '# define function: '
74
+ query_suffix: '.'
75
+ stop: ['# define', '# example']
76
+ maintain_session: False
77
+ debug_mode: False
78
+ include_context: True
79
+
80
+ tabletop_coords:
81
+ top_left: [-0.25, -0.25]
82
+ top_side: [0, -0.25]
83
+ top_right: [0.25, -0.25]
84
+ left_side: [-0.25, -0.5]
85
+ middle: [0, -0.5]
86
+ right_side: [0.25, -0.5]
87
+ bottom_left: [-0.25, -0.75]
88
+ bottom_side: [0, -0.75]
89
+ bottom_right: [0.25, -0.75]
90
+ table_z: 0
consts.py ADDED
@@ -0,0 +1,33 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import numpy as np
2
+
3
+ # # Global constants: pick and place objects, colors, workspace bounds
4
+ COLORS = {
5
+ 'blue': (78/255, 121/255, 167/255, 255/255),
6
+ 'red': (255/255, 87/255, 89/255, 255/255),
7
+ 'green': (89/255, 169/255, 79/255, 255/255),
8
+ 'orange': (242/255, 142/255, 43/255, 255/255),
9
+ 'yellow': (237/255, 201/255, 72/255, 255/255),
10
+ 'purple': (176/255, 122/255, 161/255, 255/255),
11
+ 'pink': (255/255, 157/255, 167/255, 255/255),
12
+ 'cyan': (118/255, 183/255, 178/255, 255/255),
13
+ 'brown': (156/255, 117/255, 95/255, 255/255),
14
+ 'gray': (186/255, 176/255, 172/255, 255/255),
15
+ }
16
+
17
+ CORNER_POS = {
18
+ 'top left corner': (-0.3 + 0.05, -0.2 - 0.05, 0),
19
+ 'top side': (0, -0.2 - 0.05, 0),
20
+ 'top right corner': (0.3 - 0.05, -0.2 - 0.05, 0),
21
+ 'left side': (-0.3 + 0.05, -0.5, 0),
22
+ 'middle': (0, -0.5, 0),
23
+ 'right side': (0.3 - 0.05, -0.5, 0),
24
+ 'bottom left corner': (-0.3 + 0.05, -0.8 + 0.05, 0),
25
+ 'bottom side': (0, -0.8 + 0.05, 0),
26
+ 'bottom right corner': (0.3 - 0.05, -0.8 + 0.05, 0),
27
+ }
28
+
29
+ ALL_BLOCKS = ['blue block', 'red block', 'green block', 'orange block', 'yellow block', 'purple block', 'pink block', 'cyan block', 'brown block', 'gray block']
30
+ ALL_BOWLS = ['blue bowl', 'red bowl', 'green bowl', 'orange bowl', 'yellow bowl', 'purple bowl', 'pink bowl', 'cyan bowl', 'brown bowl', 'gray bowl']
31
+
32
+ PIXEL_SIZE = 0.00267857
33
+ BOUNDS = np.float32([[-0.3, 0.3], [-0.8, -0.2], [0, 0.15]]) # X Y Z
lmp.py ADDED
@@ -0,0 +1,251 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from time import sleep
2
+ import ast
3
+ import astunparse
4
+ import openai
5
+ from openai.error import RateLimitError, APIConnectionError
6
+ from pygments import highlight
7
+ from pygments.lexers import PythonLexer
8
+ from pygments.formatters import TerminalFormatter
9
+
10
+
11
+ class LMP:
12
+
13
+ def __init__(self, name, cfg, lmp_fgen, fixed_vars, variable_vars):
14
+ self._name = name
15
+ self._cfg = cfg
16
+
17
+ with open(self._cfg['prompt_path'], 'r') as f:
18
+ self._base_prompt = f.read()
19
+
20
+ self._stop_tokens = list(self._cfg['stop'])
21
+
22
+ self._lmp_fgen = lmp_fgen
23
+
24
+ self._fixed_vars = fixed_vars
25
+ self._variable_vars = variable_vars
26
+ self.exec_hist = ''
27
+
28
+ def clear_exec_hist(self):
29
+ self.exec_hist = ''
30
+
31
+ def build_prompt(self, query, context=''):
32
+ if len(self._variable_vars) > 0:
33
+ variable_vars_imports_str = f"from utils import {', '.join(self._variable_vars.keys())}"
34
+ else:
35
+ variable_vars_imports_str = ''
36
+ prompt = self._base_prompt.replace('{variable_vars_imports}', variable_vars_imports_str)
37
+
38
+ if self._cfg['maintain_session']:
39
+ prompt += f'\n{self.exec_hist}'
40
+
41
+ if context != '':
42
+ prompt += f'\n{context}'
43
+
44
+ use_query = f'{self._cfg["query_prefix"]}{query}{self._cfg["query_suffix"]}'
45
+ prompt += f'\n{use_query}'
46
+
47
+ return prompt, use_query
48
+
49
+ def __call__(self, query, context='', **kwargs):
50
+ prompt, use_query = self.build_prompt(query, context=context)
51
+
52
+ while True:
53
+ try:
54
+ code_str = openai.Completion.create(
55
+ prompt=prompt,
56
+ stop=self._stop_tokens,
57
+ temperature=self._cfg['temperature'],
58
+ engine=self._cfg['engine'],
59
+ max_tokens=self._cfg['max_tokens']
60
+ )['choices'][0]['text'].strip()
61
+ break
62
+ except (RateLimitError, APIConnectionError) as e:
63
+ print(f'OpenAI API got err {e}')
64
+ print('Retrying after 10s.')
65
+ sleep(10)
66
+
67
+ if self._cfg['include_context'] and context != '':
68
+ to_exec = f'{context}\n{code_str}'
69
+ to_log = f'{context}\n{use_query}\n{code_str}'
70
+ else:
71
+ to_exec = code_str
72
+ to_log = f'{use_query}\n{to_exec}'
73
+
74
+ to_log_pretty = highlight(to_log, PythonLexer(), TerminalFormatter())
75
+ print(f'LMP {self._name} exec:\n\n{to_log_pretty}\n')
76
+
77
+ new_fs = self._lmp_fgen.create_new_fs_from_code(code_str)
78
+ self._variable_vars.update(new_fs)
79
+
80
+ gvars = merge_dicts([self._fixed_vars, self._variable_vars])
81
+ lvars = kwargs
82
+
83
+ if not self._cfg['debug_mode']:
84
+ exec_safe(to_exec, gvars, lvars)
85
+
86
+ self.exec_hist += f'\n{to_exec}'
87
+
88
+ if self._cfg['maintain_session']:
89
+ self._variable_vars.update(lvars)
90
+
91
+ if self._cfg['has_return']:
92
+ return lvars[self._cfg['return_val_name']]
93
+
94
+
95
+ class LMPFGen:
96
+
97
+ def __init__(self, cfg, fixed_vars, variable_vars):
98
+ self._cfg = cfg
99
+
100
+ self._stop_tokens = list(self._cfg['stop'])
101
+ self._fixed_vars = fixed_vars
102
+ self._variable_vars = variable_vars
103
+
104
+ with open(self._cfg['prompt_path'], 'r') as f:
105
+ self._base_prompt = f.read()
106
+
107
+ def create_f_from_sig(self, f_name, f_sig, other_vars=None, fix_bugs=False, return_src=False):
108
+ print(f'Creating function: {f_sig}')
109
+
110
+ use_query = f'{self._cfg["query_prefix"]}{f_sig}{self._cfg["query_suffix"]}'
111
+ prompt = f'{self._base_prompt}\n{use_query}'
112
+
113
+ while True:
114
+ try:
115
+ f_src = openai.Completion.create(
116
+ prompt=prompt,
117
+ stop=self._stop_tokens,
118
+ temperature=self._cfg['temperature'],
119
+ engine=self._cfg['engine'],
120
+ max_tokens=self._cfg['max_tokens']
121
+ )['choices'][0]['text'].strip()
122
+ break
123
+ except (RateLimitError, APIConnectionError) as e:
124
+ print(f'OpenAI API got err {e}')
125
+ print('Retrying after 10s.')
126
+ sleep(10)
127
+
128
+ if fix_bugs:
129
+ f_src = openai.Edit.create(
130
+ model='code-davinci-edit-001',
131
+ input='# ' + f_src,
132
+ temperature=0,
133
+ instruction='Fix the bug if there is one. Improve readability. Keep same inputs and outputs. Only small changes. No comments.',
134
+ )['choices'][0]['text'].strip()
135
+
136
+ if other_vars is None:
137
+ other_vars = {}
138
+ gvars = merge_dicts([self._fixed_vars, self._variable_vars, other_vars])
139
+ lvars = {}
140
+
141
+ exec_safe(f_src, gvars, lvars)
142
+
143
+ f = lvars[f_name]
144
+
145
+ to_print = highlight(f'{use_query}\n{f_src}', PythonLexer(), TerminalFormatter())
146
+ print(f'LMP FGEN created:\n\n{to_print}\n')
147
+
148
+ if return_src:
149
+ return f, f_src
150
+ return f
151
+
152
+ def create_new_fs_from_code(self, code_str, other_vars=None, fix_bugs=False, return_src=False):
153
+ fs, f_assigns = {}, {}
154
+ f_parser = FunctionParser(fs, f_assigns)
155
+ f_parser.visit(ast.parse(code_str))
156
+ for f_name, f_assign in f_assigns.items():
157
+ if f_name in fs:
158
+ fs[f_name] = f_assign
159
+
160
+ if other_vars is None:
161
+ other_vars = {}
162
+
163
+ new_fs = {}
164
+ srcs = {}
165
+ for f_name, f_sig in fs.items():
166
+ all_vars = merge_dicts([self._fixed_vars, self._variable_vars, new_fs, other_vars])
167
+ if not var_exists(f_name, all_vars):
168
+ f, f_src = self.create_f_from_sig(f_name, f_sig, new_fs, fix_bugs=fix_bugs, return_src=True)
169
+
170
+ # recursively define child_fs in the function body if needed
171
+ f_def_body = astunparse.unparse(ast.parse(f_src).body[0].body)
172
+ child_fs, child_f_srcs = self.create_new_fs_from_code(
173
+ f_def_body, other_vars=all_vars, fix_bugs=fix_bugs, return_src=True
174
+ )
175
+
176
+ if len(child_fs) > 0:
177
+ new_fs.update(child_fs)
178
+ srcs.update(child_f_srcs)
179
+
180
+ # redefine parent f so newly created child_fs are in scope
181
+ gvars = merge_dicts([self._fixed_vars, self._variable_vars, new_fs, other_vars])
182
+ lvars = {}
183
+
184
+ exec_safe(f_src, gvars, lvars)
185
+
186
+ f = lvars[f_name]
187
+
188
+ new_fs[f_name], srcs[f_name] = f, f_src
189
+
190
+ if return_src:
191
+ return new_fs, srcs
192
+ return new_fs
193
+
194
+
195
+ class FunctionParser(ast.NodeTransformer):
196
+
197
+ def __init__(self, fs, f_assigns):
198
+ super().__init__()
199
+ self._fs = fs
200
+ self._f_assigns = f_assigns
201
+
202
+ def visit_Call(self, node):
203
+ self.generic_visit(node)
204
+ if isinstance(node.func, ast.Name):
205
+ f_sig = astunparse.unparse(node).strip()
206
+ f_name = astunparse.unparse(node.func).strip()
207
+ self._fs[f_name] = f_sig
208
+ return node
209
+
210
+ def visit_Assign(self, node):
211
+ self.generic_visit(node)
212
+ if isinstance(node.value, ast.Call):
213
+ assign_str = astunparse.unparse(node).strip()
214
+ f_name = astunparse.unparse(node.value.func).strip()
215
+ self._f_assigns[f_name] = assign_str
216
+ return node
217
+
218
+
219
+ def var_exists(name, all_vars):
220
+ try:
221
+ eval(name, all_vars)
222
+ except:
223
+ exists = False
224
+ else:
225
+ exists = True
226
+ return exists
227
+
228
+
229
+ def merge_dicts(dicts):
230
+ return {
231
+ k : v
232
+ for d in dicts
233
+ for k, v in d.items()
234
+ }
235
+
236
+
237
+ def exec_safe(code_str, gvars=None, lvars=None):
238
+ banned_phrases = ['import', '__']
239
+ for phrase in banned_phrases:
240
+ assert phrase not in code_str
241
+
242
+ if gvars is None:
243
+ gvars = {}
244
+ if lvars is None:
245
+ lvars = {}
246
+ empty_fn = lambda *args, **kwargs: None
247
+ custom_gvars = merge_dicts([
248
+ gvars,
249
+ {'exec': empty_fn, 'eval': empty_fn}
250
+ ])
251
+ exec(code_str, custom_gvars, lvars)
prompts/fgen.py ADDED
@@ -0,0 +1,49 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import numpy as np
2
+ from shapely.geometry import *
3
+ from shapely.affinity import *
4
+
5
+ from env_utils import get_obj_pos, get_obj_names
6
+ from ctrl_utils import put_first_on_second
7
+
8
+ # define function: total = get_total(xs=numbers).
9
+ def get_total(xs):
10
+ return np.sum(xs)
11
+
12
+ # define function: y = eval_line(x, slope, y_intercept=0).
13
+ def eval_line(x, slope, y_intercept):
14
+ return x * slope + y_intercept
15
+
16
+ # define function: pt = get_pt_to_the_left(pt, dist).
17
+ def get_pt_to_the_left(pt, dist):
18
+ return pt + [-dist, 0]
19
+
20
+ # define function: pt = get_pt_to_the_top(pt, dist).
21
+ def get_pt_to_the_top(pt, dist):
22
+ return pt + [0, dist]
23
+
24
+ # define function line = make_line_by_length(length=x).
25
+ def make_line_by_length(length):
26
+ line = LineString([[0, 0], [length, 0]])
27
+ return line
28
+
29
+ # define function: line = make_vertical_line_by_length(length=x).
30
+ def make_vertical_line_by_length(length):
31
+ line = make_line_by_length(length)
32
+ vertical_line = rotate(line, 90)
33
+ return vertical_line
34
+
35
+ # define function: pt = interpolate_line(line, t=0.5).
36
+ def interpolate_line(line, t):
37
+ pt = line.interpolate(t, normalized=True)
38
+ return np.array(pt.coords[0])
39
+
40
+ # example: scale a line by 2.
41
+ line = make_line_by_length(1)
42
+ new_shape = scale(line, xfact=2, yfact=2)
43
+
44
+ # example: put object1 on top of object0.
45
+ put_first_on_second('object1', 'object0')
46
+
47
+ # example: get the position of the first object.
48
+ obj_names = get_obj_names()
49
+ pos_2d = get_obj_pos(obj_names[0])
prompts/parse_obj_name.py ADDED
@@ -0,0 +1,64 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import numpy as np
2
+ from env_utils import get_obj_pos, parse_position
3
+ from utils import get_obj_positions_np
4
+
5
+ objects = ['blue block', 'cyan block', 'purple bowl', 'gray bowl', 'brown bowl', 'pink block', 'purple block']
6
+ # the block closest to the purple bowl.
7
+ block_names = ['blue block', 'cyan block', 'purple block']
8
+ block_positions = get_obj_positions_np(block_names)
9
+ closest_block_idx = get_closest_idx(points=block_positions, point=get_obj_pos('purple bowl'))
10
+ closest_block_name = block_names[closest_block_idx]
11
+ ret_val = closest_block_name
12
+ objects = ['brown bowl', 'banana', 'brown block', 'apple', 'blue bowl', 'blue block']
13
+ # the blocks.
14
+ ret_val = ['brown block', 'blue block']
15
+ objects = ['brown bowl', 'banana', 'brown block', 'apple', 'blue bowl', 'blue block']
16
+ # the brown objects.
17
+ ret_val = ['brown bowl', 'brown block']
18
+ objects = ['brown bowl', 'banana', 'brown block', 'apple', 'blue bowl', 'blue block']
19
+ # a fruit that's not the apple
20
+ fruit_names = ['banana', 'apple']
21
+ for fruit_name in fruit_names:
22
+ if fruit_name != 'apple':
23
+ ret_val = fruit_name
24
+ objects = ['blue block', 'cyan block', 'purple bowl', 'brown bowl', 'purple block']
25
+ # blocks above the brown bowl.
26
+ block_names = ['blue block', 'cyan block', 'purple block']
27
+ brown_bowl_pos = get_obj_pos('brown bowl')
28
+ use_block_names = []
29
+ for block_name in block_names:
30
+ if get_obj_pos(block_name)[1] > brown_bowl_pos[1]:
31
+ use_block_names.append(block_name)
32
+ ret_val = use_block_names
33
+ objects = ['blue block', 'cyan block', 'purple bowl', 'brown bowl', 'purple block']
34
+ # the blue block.
35
+ ret_val = 'blue block'
36
+ objects = ['blue block', 'cyan block', 'purple bowl', 'brown bowl', 'purple block']
37
+ # the block closest to the bottom right corner.
38
+ corner_pos = parse_position('bottom right corner')
39
+ block_names = ['blue block', 'cyan block', 'purple block']
40
+ block_positions = get_obj_positions_np(block_names)
41
+ closest_block_idx = get_closest_idx(points=block_positions, point=corner_pos)
42
+ closest_block_name = block_names[closest_block_idx]
43
+ ret_val = closest_block_name
44
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
45
+ # the left most block.
46
+ block_names = ['green block', 'brown block', 'blue block']
47
+ block_positions = get_obj_positions_np(block_names)
48
+ left_block_idx = np.argsort(block_positions[:, 0])[0]
49
+ left_block_name = block_names[left_block_idx]
50
+ ret_val = left_block_name
51
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
52
+ # the bowl on near the top.
53
+ bowl_names = ['brown bowl', 'green bowl', 'blue bowl']
54
+ bowl_positions = get_obj_positions_np(bowl_names)
55
+ top_bowl_idx = np.argsort(block_positions[:, 1])[-1]
56
+ top_bowl_name = bowl_names[top_bowl_idx]
57
+ ret_val = top_bowl_name
58
+ objects = ['yellow bowl', 'purple block', 'yellow block', 'purple bowl', 'pink bowl', 'pink block']
59
+ # the third bowl from the right.
60
+ bowl_names = ['yellow bowl', 'purple bowl', 'pink bowl']
61
+ bowl_positions = get_obj_positions_np(bowl_names)
62
+ bowl_idx = np.argsort(block_positions[:, 0])[-3]
63
+ bowl_name = bowl_names[bowl_idx]
64
+ ret_val = bowl_name
prompts/parse_position.py ADDED
@@ -0,0 +1,54 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import numpy as np
2
+ from shapely.geometry import *
3
+ from shapely.affinity import *
4
+ from env_utils import denormalize_xy, parse_obj_name, get_obj_names, get_obj_pos
5
+
6
+ # a 30cm horizontal line in the middle with 3 points.
7
+ middle_pos = denormalize_xy([0.5, 0.5])
8
+ start_pos = middle_pos + [-0.3/2, 0]
9
+ end_pos = middle_pos + [0.3/2, 0]
10
+ line = make_line(start=start_pos, end=end_pos)
11
+ points = interpolate_pts_on_line(line=line, n=3)
12
+ ret_val = points
13
+ # a 20cm vertical line near the right with 4 points.
14
+ middle_pos = denormalize_xy([1, 0.5])
15
+ start_pos = middle_pos + [0, -0.2/2]
16
+ end_pos = middle_pos + [0, 0.2/2]
17
+ line = make_line(start=start_pos, end=end_pos)
18
+ points = interpolate_pts_on_line(line=line, n=4)
19
+ ret_val = points
20
+ # a diagonal line from the top left to the bottom right corner with 5 points.
21
+ top_left_corner = denormalize_xy([0, 1])
22
+ bottom_right_corner = denormalize_xy([1, 0])
23
+ line = make_line(start=top_left_corner, end=bottom_right_corner)
24
+ points = interpolate_pts_on_line(line=line, n=5)
25
+ ret_val = points
26
+ # a triangle with size 10cm with 3 points.
27
+ polygon = make_triangle(size=0.1, center=denormalize_xy([0.5, 0.5]))
28
+ points = get_points_from_polygon(polygon)
29
+ ret_val = points
30
+ # the corner closest to the sun colored block.
31
+ block_name = parse_obj_name('the sun colored block', f'objects = {get_obj_names()}')
32
+ corner_positions = np.array([denormalize_xy(pos) for pos in [[0, 0], [0, 1], [1, 1], [1, 0]]])
33
+ closest_corner_pos = get_closest_point(points=corner_positions, point=get_obj_pos(block_name))
34
+ ret_val = closest_corner_pos
35
+ # the side farthest from the right most bowl.
36
+ bowl_name = parse_obj_name('the right most bowl', f'objects = {get_obj_names()}')
37
+ side_positions = np.array([denormalize_xy(pos) for pos in [[0.5, 0], [0.5, 1], [1, 0.5], [0, 0.5]]])
38
+ farthest_side_pos = get_farthest_point(points=side_positions, point=get_obj_pos(bowl_name))
39
+ ret_val = farthest_side_pos
40
+ # a point above the third block from the bottom.
41
+ block_name = parse_obj_name('the third block from the bottom', f'objects = {get_obj_names()}')
42
+ ret_val = get_obj_pos(block_name) + [0.1, 0]
43
+ # a point 10cm left of the bowls.
44
+ bowl_names = parse_obj_name('the bowls', f'objects = {get_obj_names()}')
45
+ bowl_positions = get_all_object_positions_np(obj_names=bowl_names)
46
+ left_obj_pos = bowl_positions[np.argmin(bowl_positions[:, 0])] + [-0.1, 0]
47
+ ret_val = left_obj_pos
48
+ # the bottom side.
49
+ bottom_pos = denormalize_xy([0.5, 0])
50
+ ret_val = bottom_pos
51
+ # the top corners.
52
+ top_left_pos = denormalize_xy([0, 1])
53
+ top_right_pos = denormalize_xy([1, 1])
54
+ ret_val = [top_left_pos, top_right_pos]
prompts/parse_question.py ADDED
@@ -0,0 +1,30 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from utils import get_obj_pos, get_obj_names, parse_obj_name, bbox_contains_pt, is_obj_visible
2
+
3
+ objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl', 'fruit', 'green block', 'black bowl']
4
+ # is the blue block to the right of the yellow bowl?
5
+ ret_val = get_obj_pos('blue block')[0] > get_obj_pos('yellow bowl')[0]
6
+ objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl', 'fruit', 'green block', 'black bowl']
7
+ # how many yellow objects are there?
8
+ yellow_object_names = parse_obj_name('the yellow objects', f'objects = {get_obj_names()}')
9
+ ret_val = len(yellow_object_names)
10
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
11
+ # is the pink block on the green bowl?
12
+ ret_val = bbox_contains_pt(container_name='green bowl', obj_name='pink block')
13
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
14
+ # what are the blocks left of the green bowl?
15
+ block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
16
+ green_bowl_pos = get_obj_pos('green bowl')
17
+ left_block_names = []
18
+ for block_name in block_names:
19
+ if get_obj_pos(block_name)[0] < green_bowl_pos[0]:
20
+ left_block_names.append(block_name)
21
+ ret_val = left_block_names
22
+ objects = ['pink block', 'yellow block', 'pink bowl', 'blue block', 'blue bowl', 'yellow bowl']
23
+ # is the sun colored block above the blue bowl?
24
+ sun_block_name = parse_obj_name('sun colored block', f'objects = {get_obj_names()}')
25
+ sun_block_pos = get_obj_pos(sun_block_name)
26
+ blue_bowl_pos = get_obj_pos('blue bowl')
27
+ ret_val = sun_block_pos[1] > blue_bowl_pos[1]
28
+ objects = ['pink block', 'yellow block', 'pink bowl', 'blue block', 'blue bowl', 'yellow bowl']
29
+ # is the green block below the blue bowl?
30
+ ret_val = get_obj_pos('green block')[1] < get_obj_pos('blue bowl')[1]
prompts/tabletop_ui.py ADDED
@@ -0,0 +1,198 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Python 2D robot control script
2
+ import numpy as np
3
+ 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
4
+ from plan_utils import parse_obj_name, parse_position, parse_question, transform_shape_pts
5
+
6
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
7
+ # the yellow block on the yellow bowl.
8
+ say('Ok - putting the yellow block on the yellow bowl')
9
+ put_first_on_second('yellow block', 'yellow bowl')
10
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
11
+ # which block did you move.
12
+ say('I moved the yellow block')
13
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
14
+ # move the green block to the top right corner.
15
+ say('Got it - putting the green block on the top right corner')
16
+ corner_pos = parse_position('top right corner')
17
+ put_first_on_second('green block', corner_pos)
18
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
19
+ # stack the blue bowl on the yellow bowl on the green block.
20
+ order_bottom_to_top = ['green block', 'yellow block', 'blue bowl']
21
+ say(f'Sure - stacking from top to bottom: {", ".join(order_bottom_to_top)}')
22
+ stack_objects_in_order(object_names=order_bottom_to_top)
23
+ objects = ['cyan block', 'white block', 'cyan bowl', 'blue block', 'blue bowl', 'white bowl']
24
+ # move the cyan block into its corresponding bowl.
25
+ matches = {'cyan block': 'cyan bowl'}
26
+ say('Got it - placing the cyan block on the cyan bowl')
27
+ for first, second in matches.items():
28
+ put_first_on_second(first, get_obj_pos(second))
29
+ objects = ['cyan block', 'white block', 'cyan bowl', 'blue block', 'blue bowl', 'white bowl']
30
+ # make a line of blocks on the right side.
31
+ say('No problem! Making a line of blocks on the right side')
32
+ block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
33
+ line_pts = parse_position(f'a 30cm vertical line on the right with {len(block_names)} points')
34
+ for block_name, pt in zip(block_names, line_pts):
35
+ put_first_on_second(block_name, pt)
36
+ objects = ['yellow block', 'red block', 'yellow bowl', 'gray block', 'gray bowl', 'red bowl']
37
+ # put the small banana colored thing in between the blue bowl and green block.
38
+ say('Sure thing - putting the yellow block between the blue bowl and the green block')
39
+ target_pos = parse_position('a point in the middle betweeen the blue bowl and the green block')
40
+ put_first_on_second('yellow block', target_pos)
41
+ objects = ['yellow block', 'red block', 'yellow bowl', 'gray block', 'gray bowl', 'red bowl']
42
+ # can you cut the bowls in half.
43
+ say('no, I can only move objects around')
44
+ objects = ['yellow block', 'green block', 'yellow bowl', 'gray block', 'gray bowl', 'green bowl']
45
+ # stack the blocks on the right side with the gray one on the bottom.
46
+ say('Ok. stacking the blocks on the right side with the gray block on the bottom')
47
+ right_side = parse_position('the right side')
48
+ put_first_on_second('gray block', right_side)
49
+ order_bottom_to_top = ['gray block', 'green block', 'yellow block']
50
+ stack_objects_in_order(object_names=order_bottom_to_top)
51
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
52
+ # hide the blue bowl.
53
+ bowl_name = np.random.choice(['yellow bowl', 'green bowl'])
54
+ say(f'Sounds good! Hiding the blue bowl under the {bowl_name}')
55
+ put_first_on_second(bowl_name, 'blue bowl')
56
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
57
+ # stack everything with the green block on top.
58
+ say('Ok! Stacking everything with the green block on the top')
59
+ order_bottom_to_top = ['blue bowl', 'pink bowl', 'green bowl', 'pink block', 'blue block', 'green block']
60
+ stack_objects_in_order(object_names=order_bottom_to_top)
61
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
62
+ # move the grass-colored bowl to the left.
63
+ say('Sure - moving the green bowl left by 10 centimeters')
64
+ left_pos = parse_position('a point 10cm left of the green bowl')
65
+ put_first_on_second('green bowl', left_pos)
66
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
67
+ # why did you move the red bowl.
68
+ say(f'I did not move the red bowl')
69
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
70
+ # undo that.
71
+ say('Sure - moving the green bowl right by 10 centimeters')
72
+ left_pos = parse_position('a point 10cm right of the green bowl')
73
+ put_first_on_second('green bowl', left_pos)
74
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
75
+ # place the top most block to the corner closest to the bottom most block.
76
+ top_block_name = parse_obj_name('top most block', f'objects = {get_obj_names()}')
77
+ bottom_block_name = parse_obj_name('bottom most block', f'objects = {get_obj_names()}')
78
+ closest_corner_pos = parse_position(f'the corner closest to the {bottom_block_name}', f'objects = {get_obj_names()}')
79
+ say(f'Putting the {top_block_name} on the {get_corner_name(closest_corner_pos)}')
80
+ put_first_on_second(top_block_name, closest_corner_pos)
81
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
82
+ # move the brown bowl to the side closest to the green block.
83
+ closest_side_position = parse_position('the side closest to the green block')
84
+ say(f'Got it - putting the brown bowl on the {get_side_name(closest_side_position)}')
85
+ put_first_on_second('brown bowl', closest_side_position)
86
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
87
+ # place the green block to the right of the bowl that has the blue block.
88
+ bowl_name = parse_obj_name('the bowl that has the blue block', f'objects = {get_obj_names()}')
89
+ if bowl_name:
90
+ target_pos = parse_position(f'a point 10cm to the right of the {bowl_name}')
91
+ say(f'No problem - placing the green block to the right of the {bowl_name}')
92
+ put_first_on_second('green block', target_pos)
93
+ else:
94
+ say('There are no bowls that has the blue block')
95
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
96
+ # place the blue block in the empty bowl.
97
+ empty_bowl_name = parse_obj_name('the empty bowl', f'objects = {get_obj_names()}')
98
+ if empty_bowl_name:
99
+ say(f'Ok! Putting the blue block on the {empty_bowl_name}')
100
+ put_first_on_second('blue block', empty_bowl_name)
101
+ else:
102
+ say('There are no empty bowls')
103
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
104
+ # move the other blocks to the bottom corners.
105
+ block_names = parse_obj_name('blocks other than the blue block', f'objects = {get_obj_names()}')
106
+ corners = parse_position('the bottom corners')
107
+ for block_name, pos in zip(block_names, corners):
108
+ put_first_on_second(block_name, pos)
109
+ objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
110
+ # move the red bowl a lot to the left of the blocks.
111
+ say('Sure! Moving the red bowl to a point left of the blocks')
112
+ left_pos = parse_position('a point 20cm left of the blocks')
113
+ put_first_on_second('red bowl', left_pos)
114
+ objects = ['pink block', 'gray block', 'orange block']
115
+ # move the pinkish colored block on the bottom side.
116
+ say('Ok - putting the pink block on the bottom side')
117
+ bottom_side_pos = parse_position('the bottom side')
118
+ put_first_on_second('pink block', bottom_side_pos)
119
+ objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl']
120
+ # is the blue block to the right of the yellow bowl?
121
+ if parse_question('is the blue block to the right of the yellow bowl?', f'objects = {get_obj_names()}'):
122
+ say('yes, there is a blue block to the right of the yellow bow')
123
+ else:
124
+ say('no, there is\'t a blue block to the right of the yellow bow')
125
+ objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl']
126
+ # how many yellow objects are there?
127
+ n_yellow_objs = parse_question('how many yellow objects are there', f'objects = {get_obj_names()}')
128
+ say(f'there are {n_yellow_objs} yellow object')
129
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
130
+ # move the left most block to the green bowl.
131
+ left_block_name = parse_obj_name('left most block', f'objects = {get_obj_names()}')
132
+ say(f'Moving the {left_block_name} on the green bowl')
133
+ put_first_on_second(left_block_name, 'green bowl')
134
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
135
+ # move the other blocks to different corners.
136
+ block_names = parse_obj_name(f'blocks other than the {left_block_name}', f'objects = {get_obj_names()}')
137
+ corners = parse_position('the corners')
138
+ say(f'Ok - moving the other {len(block_names)} blocks to different corners')
139
+ for block_name, pos in zip(block_names, corners):
140
+ put_first_on_second(block_name, pos)
141
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
142
+ # is the pink block on the green bowl.
143
+ if parse_question('is the pink block on the green bowl', f'objects = {get_obj_names()}'):
144
+ say('Yes - the pink block is on the green bowl.')
145
+ else:
146
+ say('No - the pink block is not on the green bowl.')
147
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
148
+ # what are the blocks left of the green bowl.
149
+ left_block_names = parse_question('what are the blocks left of the green bowl', f'objects = {get_obj_names()}')
150
+ if len(left_block_names) > 0:
151
+ say(f'These blocks are left of the green bowl: {", ".join(left_block_names)}')
152
+ else:
153
+ say('There are no blocks left of the green bowl')
154
+ objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
155
+ # if you see a purple bowl put it on the blue bowl
156
+ if is_obj_visible('purple bowl'):
157
+ say('Putting the purple bowl on the pink bowl')
158
+ put_first_on_second('purple bowl', 'pink bowl')
159
+ else:
160
+ say('I don\'t see a purple bowl')
161
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
162
+ # imagine that the bowls are different biomes on earth and imagine that the blocks are parts of a building.
163
+ say('ok')
164
+ objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
165
+ # now build a tower in the grasslands.
166
+ order_bottom_to_top = ['green bowl', 'blue block', 'green block', 'yellow block']
167
+ say('stacking the blocks on the green bowl')
168
+ stack_objects_in_order(object_names=order_bottom_to_top)
169
+ objects = ['yellow block', 'green block', 'yellow bowl', 'gray block', 'gray bowl', 'green bowl']
170
+ # show me what happens when the desert gets flooded by the ocean.
171
+ say('putting the yellow bowl on the blue bowl')
172
+ put_first_on_second('yellow bowl', 'blue bowl')
173
+ objects = ['pink block', 'gray block', 'orange block']
174
+ # move all blocks 5cm toward the top.
175
+ say('Ok - moving all blocks 5cm toward the top')
176
+ block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
177
+ for block_name in block_names:
178
+ target_pos = parse_position(f'a point 5cm above the {block_name}')
179
+ put_first_on_second(block_name, target_pos)
180
+ objects = ['cyan block', 'white block', 'purple bowl', 'blue block', 'blue bowl', 'white bowl']
181
+ # make a triangle of blocks in the middle.
182
+ block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
183
+ triangle_pts = parse_position(f'a triangle with size 10cm around the middle with {len(block_names)} points')
184
+ say('Making a triangle of blocks around the middle of the workspace')
185
+ for block_name, pt in zip(block_names, triangle_pts):
186
+ put_first_on_second(block_name, pt)
187
+ objects = ['cyan block', 'white block', 'purple bowl', 'blue block', 'blue bowl', 'white bowl']
188
+ # make the triangle smaller.
189
+ triangle_pts = transform_shape_pts('scale it by 0.5x', shape_pts=triangle_pts)
190
+ say('Making the triangle smaller')
191
+ block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
192
+ for block_name, pt in zip(block_names, triangle_pts):
193
+ put_first_on_second(block_name, pt)
194
+ objects = ['brown bowl', 'red block', 'brown block', 'red bowl', 'pink bowl', 'pink block']
195
+ # put the red block on the farthest bowl.
196
+ farthest_bowl_name = parse_obj_name('the bowl farthest from the red block', f'objects = {get_obj_names()}')
197
+ say(f'Putting the red block on the {farthest_bowl_name}')
198
+ put_first_on_second('red block', farthest_bowl_name)
prompts/transform_shape_pts.py ADDED
@@ -0,0 +1,19 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import numpy as np
2
+ from utils import get_obj_pos, get_obj_names, parse_position, parse_obj_name
3
+
4
+ # make it bigger by 1.5.
5
+ new_shape_pts = scale_pts_around_centroid_np(shape_pts, scale_x=1.5, scale_y=1.5)
6
+ # move it to the right by 10cm.
7
+ new_shape_pts = translate_pts_np(shape_pts, delta=[0.1, 0])
8
+ # move it to the top by 20cm.
9
+ new_shape_pts = translate_pts_np(shape_pts, delta=[0, 0.2])
10
+ # rotate it clockwise by 40 degrees.
11
+ new_shape_pts = rotate_pts_around_centroid_np(shape_pts, angle=-np.deg2rad(40))
12
+ # rotate by 30 degrees and make it slightly smaller
13
+ new_shape_pts = rotate_pts_around_centroid_np(shape_pts, angle=np.deg2rad(30))
14
+ new_shape_pts = scale_pts_around_centroid_np(new_shape_pts, scale_x=0.7, scale_y=0.7)
15
+ # move it toward the blue block.
16
+ block_name = parse_obj_name('the blue block', f'objects = {get_obj_names()}')
17
+ block_pos = get_obj_pos(block_name)
18
+ mean_delta = np.mean(block_pos - shape_pts, axis=1)
19
+ new_shape_pts = translate_pts_np(shape_pts, mean_delta)
requirements.txt ADDED
@@ -0,0 +1,11 @@
 
 
 
 
 
 
 
 
 
 
 
 
1
+ numpy
2
+ scipy
3
+ shapely
4
+ astunparse
5
+ pygments
6
+ openai
7
+ pybullet
8
+ imageio==2.4.1
9
+ imageio-ffmpeg
10
+ moviepy
11
+ omegaconf
robotiq_2f_85/README.md ADDED
@@ -0,0 +1,52 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ ## Robotiq 2F 85 gripper
2
+ For this gripper, the following Github repo can be used as a reference: https://github.com/Shreeyak/robotiq.git
3
+
4
+ ### mimic tag in URDF
5
+ 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:
6
+
7
+ ```python
8
+ #a mimic joint can act as a gear between two joints
9
+ #you can control the gear ratio in magnitude and sign (>0 reverses direction)
10
+
11
+ import pybullet as p
12
+ import time
13
+ p.connect(p.GUI)
14
+ p.loadURDF("plane.urdf",0,0,-2)
15
+ wheelA = p.loadURDF("differential/diff_ring.urdf",[0,0,0])
16
+ for i in range(p.getNumJoints(wheelA)):
17
+ print(p.getJointInfo(wheelA,i))
18
+ p.setJointMotorControl2(wheelA,i,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
19
+
20
+
21
+ c = p.createConstraint(wheelA,1,wheelA,3,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
22
+ p.changeConstraint(c,gearRatio=1, maxForce=10000)
23
+
24
+ c = p.createConstraint(wheelA,2,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
25
+ p.changeConstraint(c,gearRatio=-1, maxForce=10000)
26
+
27
+ c = p.createConstraint(wheelA,1,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
28
+ p.changeConstraint(c,gearRatio=-1, maxForce=10000)
29
+
30
+
31
+ p.setRealTimeSimulation(1)
32
+ while(1):
33
+ p.setGravity(0,0,-10)
34
+ time.sleep(0.01)
35
+ #p.removeConstraint(c)
36
+
37
+ ```
38
+
39
+
40
+ Details on `createConstraint` can be found in the pybullet [getting started](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.fq749wu22x4c) guide.
41
+
42
+ ### Files in folder
43
+ 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:
44
+ ```
45
+ rosrun xacro xacro --inorder simple_rq2f85_pybullet.urdf.xacro
46
+ adaptive_transmission:="true" > robotiq_2f_85_mimic_joints.urdf
47
+ ```
48
+
49
+ The URDF meant for use in pybullet is `robotiq_2f_85.urdf` and it is generated in a similar manner as above by running:
50
+ ```
51
+ rosrun xacro xacro --inorder simple_rq2f85_pybullet.urdf.xacro > robotiq_2f_85.urdf
52
+ ```
robotiq_2f_85/robotiq-2f-base.mtl ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Blender MTL File: 'gripper-2f.blend'
2
+ # Material Count: 1
3
+
4
+ newmtl Default
5
+ Ns 96.078431
6
+ Ka 1.000000 1.000000 1.000000
7
+ Kd 0.640000 0.640000 0.640000
8
+ Ks 0.500000 0.500000 0.500000
9
+ Ke 0.000000 0.000000 0.000000
10
+ Ni 1.000000
11
+ d 1.000000
12
+ illum 2
13
+ map_Kd textures/gripper-2f_BaseColor.jpg
robotiq_2f_85/robotiq-2f-base.obj ADDED
The diff for this file is too large to render. See raw diff
 
robotiq_2f_85/robotiq-2f-base.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:1c7b9f2bd92d705fc4e897c94e905973a3c05f406845e942229433deb7041453
3
+ size 1712484
robotiq_2f_85/robotiq-2f-coupler.mtl ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Blender MTL File: 'gripper-2f.blend'
2
+ # Material Count: 1
3
+
4
+ newmtl Default
5
+ Ns 96.078431
6
+ Ka 1.000000 1.000000 1.000000
7
+ Kd 0.640000 0.640000 0.640000
8
+ Ks 0.500000 0.500000 0.500000
9
+ Ke 0.000000 0.000000 0.000000
10
+ Ni 1.000000
11
+ d 1.000000
12
+ illum 2
13
+ map_Kd textures/gripper-2f_BaseColor.jpg
robotiq_2f_85/robotiq-2f-coupler.obj ADDED
The diff for this file is too large to render. See raw diff
 
robotiq_2f_85/robotiq-2f-coupler.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:d5ee95e62f8415bf5b6e503c831a958f5fc1990bf9b2865329ec38a28932727c
3
+ size 89084
robotiq_2f_85/robotiq-2f-driver.mtl ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Blender MTL File: 'gripper-2f.blend'
2
+ # Material Count: 1
3
+
4
+ newmtl Default
5
+ Ns 96.078431
6
+ Ka 1.000000 1.000000 1.000000
7
+ Kd 0.640000 0.640000 0.640000
8
+ Ks 0.500000 0.500000 0.500000
9
+ Ke 0.000000 0.000000 0.000000
10
+ Ni 1.000000
11
+ d 1.000000
12
+ illum 2
13
+ map_Kd textures/gripper-2f_BaseColor.jpg
robotiq_2f_85/robotiq-2f-driver.obj ADDED
The diff for this file is too large to render. See raw diff
 
robotiq_2f_85/robotiq-2f-driver.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:bbd6e868b1778bead60d2516c7ede581d7ad4e431744b1828757d6ea5129c112
3
+ size 67084
robotiq_2f_85/robotiq-2f-follower.mtl ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Blender MTL File: 'gripper-2f.blend'
2
+ # Material Count: 1
3
+
4
+ newmtl Default
5
+ Ns 96.078431
6
+ Ka 1.000000 1.000000 1.000000
7
+ Kd 0.640000 0.640000 0.640000
8
+ Ks 0.500000 0.500000 0.500000
9
+ Ke 0.000000 0.000000 0.000000
10
+ Ni 1.000000
11
+ d 1.000000
12
+ illum 2
13
+ map_Kd textures/gripper-2f_BaseColor.jpg
robotiq_2f_85/robotiq-2f-follower.obj ADDED
The diff for this file is too large to render. See raw diff
 
robotiq_2f_85/robotiq-2f-follower.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:633793332c081641ce200df27a40643cc293b29956e3cb5cb29cb811c33ef1c7
3
+ size 110484
robotiq_2f_85/robotiq-2f-pad.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:6c4e73a07a7d6853777450d0019691d2d43f50c9fa0bd6b0189d4af164a0ce4b
3
+ size 684
robotiq_2f_85/robotiq-2f-spring_link.mtl ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Blender MTL File: 'gripper-2f.blend'
2
+ # Material Count: 1
3
+
4
+ newmtl Default
5
+ Ns 96.078431
6
+ Ka 1.000000 1.000000 1.000000
7
+ Kd 0.640000 0.640000 0.640000
8
+ Ks 0.500000 0.500000 0.500000
9
+ Ke 0.000000 0.000000 0.000000
10
+ Ni 1.000000
11
+ d 1.000000
12
+ illum 2
13
+ map_Kd textures/gripper-2f_BaseColor.jpg
robotiq_2f_85/robotiq-2f-spring_link.obj ADDED
The diff for this file is too large to render. See raw diff
 
robotiq_2f_85/robotiq-2f-spring_link.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:4fb74a8a0d76c0e471cf19fd48bc676fd5b19123798e7a39eb6aa56869354283
3
+ size 84884
robotiq_2f_85/robotiq_2f_85.urdf ADDED
@@ -0,0 +1,299 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ <?xml version="1.0" ?>
2
+ <!-- =================================================================================== -->
3
+ <!-- | This document was autogenerated by xacro from simple_rq2f85_pybullet.urdf.xacro | -->
4
+ <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
5
+ <!-- =================================================================================== -->
6
+ <robot name="example" xmlns:xacro="http://ros.org/wiki/xacro">
7
+ <material name="Blanc">
8
+ <color rgba="1.0 1.0 1.0 1.0"/>
9
+ </material>
10
+ <link name="base_link">
11
+ <origin xyz="0 0 0"/>
12
+ <inertial>
13
+ <mass value="0.001"/>
14
+ <origin rpy="0 0 0" xyz="0 0 0"/>
15
+ <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
16
+ </inertial>
17
+ </link>
18
+ <!-- base -->
19
+ <joint name="base_link_robotiq_2f_85_base_joint" type="fixed">
20
+ <origin rpy="0 0 0" xyz="0 0 0"/>
21
+ <parent link="base_link"/>
22
+ <child link="robotiq_2f_85_base"/>
23
+ </joint>
24
+ <link name="robotiq_2f_85_base">
25
+ <inertial>
26
+ <mass value="0.1"/>
27
+ <origin xyz="0 0 0.055"/>
28
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
29
+ </inertial>
30
+ <visual>
31
+ <geometry>
32
+ <mesh filename="robotiq-2f-base.obj" scale="0.1 0.1 0.1"/>
33
+ </geometry>
34
+ <material name="Blanc"/>
35
+ </visual>
36
+ <!-- <collision>
37
+ <geometry>
38
+ <mesh filename="robotiq-2f-base.stl" scale="0.001 0.001 0.001"/>
39
+ </geometry>
40
+ </collision> -->
41
+ </link>
42
+ <!-- right finger -->
43
+ <joint name="robotiq_2f_85_right_driver_joint" type="revolute">
44
+ <origin rpy="0 0 0" xyz="0 0.0306011 0.054904"/>
45
+ <parent link="robotiq_2f_85_base"/>
46
+ <child link="robotiq_2f_85_right_driver"/>
47
+ <axis xyz="1 0 0"/>
48
+ <limit effort="60" lower="0.0" upper="0.834" velocity="1.91986177778"/>
49
+ </joint>
50
+ <link name="robotiq_2f_85_right_driver">
51
+ <inertial>
52
+ <mass value="0.1"/>
53
+ <origin xyz="0 0 0.055"/>
54
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
55
+ </inertial>
56
+ <visual>
57
+ <geometry>
58
+ <mesh filename="robotiq-2f-driver.obj" scale="0.1 0.1 0.1"/>
59
+ </geometry>
60
+ <material name="Blanc"/>
61
+ </visual>
62
+ <!-- <collision>
63
+ <geometry>
64
+ <mesh filename="robotiq-2f-driver.stl" scale="0.001 0.001 0.001"/>
65
+ </geometry>
66
+ </collision> -->
67
+ </link>
68
+ <joint name="robotiq_2f_85_right_coupler_joint" type="fixed">
69
+ <origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
70
+ <parent link="robotiq_2f_85_right_driver"/>
71
+ <child link="robotiq_2f_85_right_coupler"/>
72
+ </joint>
73
+ <link name="robotiq_2f_85_right_coupler">
74
+ <inertial>
75
+ <mass value="0.1"/>
76
+ <origin xyz="0 0 0.055"/>
77
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
78
+ </inertial>
79
+ <visual>
80
+ <geometry>
81
+ <mesh filename="robotiq-2f-coupler.obj" scale="0.1 0.1 0.1"/>
82
+ </geometry>
83
+ <material name="Blanc"/>
84
+ </visual>
85
+ <!-- <collision>
86
+ <geometry>
87
+ <mesh filename="robotiq-2f-coupler.stl" scale="0.001 0.001 0.001"/>
88
+ </geometry>
89
+ </collision> -->
90
+ </link>
91
+ <joint name="robotiq_2f_85_right_follower_joint" type="revolute">
92
+ <origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
93
+ <parent link="robotiq_2f_85_right_coupler"/>
94
+ <child link="robotiq_2f_85_right_follower"/>
95
+ <axis xyz="1 0 0"/>
96
+ <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
97
+ <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="-1"/>
98
+ </joint>
99
+ <link name="robotiq_2f_85_right_follower">
100
+ <visual>
101
+ <geometry>
102
+ <mesh filename="robotiq-2f-follower.obj" scale="0.1 0.1 0.1"/>
103
+ </geometry>
104
+ <material name="Blanc"/>
105
+ </visual>
106
+ <!-- <collision>
107
+ <geometry>
108
+ <mesh filename="robotiq-2f-follower.stl" scale="0.001 0.001 0.001"/>
109
+ </geometry>
110
+ </collision> -->
111
+ <inertial>
112
+ <mass value="0.1"/>
113
+ <origin xyz="0 0 0.055"/>
114
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
115
+ </inertial>
116
+ </link>
117
+ <joint name="robotiq_2f_85_right_pad_joint" type="fixed">
118
+ <parent link="robotiq_2f_85_right_follower"/>
119
+ <child link="robotiq_2f_85_right_pad"/>
120
+ </joint>
121
+ <link name="robotiq_2f_85_right_pad">
122
+ <inertial>
123
+ <mass value="0.1"/>
124
+ <origin xyz="0 0 0.055" />
125
+ <inertia ixx="0.00019" iyy="0.00018" izz="0.00019" ixy="0" iyz="0" ixz="0"/>
126
+ </inertial>
127
+ <visual>
128
+ <geometry>
129
+ <mesh filename="robotiq-2f-pad.stl" scale="0.0001 0.0001 0.0001"/>
130
+ </geometry>
131
+ <material name="Blanc"/>
132
+ </visual>
133
+ <collision>
134
+ <geometry>
135
+ <mesh filename="robotiq-2f-pad.stl" scale="0.001 0.001 0.001"/>
136
+ </geometry>
137
+ </collision>
138
+ </link>
139
+ <joint name="robotiq_2f_85_right_spring_link_joint" type="revolute">
140
+ <origin rpy="0 0 0" xyz="0 0.012 0.0614"/>
141
+ <parent link="robotiq_2f_85_base"/>
142
+ <child link="robotiq_2f_85_right_spring_link"/>
143
+ <axis xyz="1 0 0"/>
144
+ <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
145
+ <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="1"/>
146
+ </joint>
147
+ <link name="robotiq_2f_85_right_spring_link">
148
+ <inertial>
149
+ <mass value="0.1"/>
150
+ <origin xyz="0 0 0.055"/>
151
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
152
+ </inertial>
153
+ <visual>
154
+ <geometry>
155
+ <mesh filename="robotiq-2f-spring_link.obj" scale="0.1 0.1 0.1"/>
156
+ </geometry>
157
+ <material name="Blanc"/>
158
+ </visual>
159
+ <!-- <collision>
160
+ <geometry>
161
+ <mesh filename="robotiq-2f-spring_link.stl" scale="0.001 0.001 0.001"/>
162
+ </geometry>
163
+ </collision> -->
164
+ </link>
165
+ <!-- left finger -->
166
+ <joint name="robotiq_2f_85_left_driver_joint" type="revolute">
167
+ <origin rpy="0 0 3.141592653589793" xyz="0 -0.0306011 0.054904"/>
168
+ <parent link="robotiq_2f_85_base"/>
169
+ <child link="robotiq_2f_85_left_driver"/>
170
+ <axis xyz="1 0 0"/>
171
+ <limit effort="176" lower="0.0" upper="0.834" velocity="1.91986177778"/>
172
+ <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="1"/>
173
+ </joint>
174
+ <link name="robotiq_2f_85_left_driver">
175
+ <inertial>
176
+ <mass value="0.1"/>
177
+ <origin xyz="0 0 0.055"/>
178
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
179
+ </inertial>
180
+ <visual>
181
+ <geometry>
182
+ <mesh filename="robotiq-2f-driver.obj" scale="0.1 0.1 0.1"/>
183
+ </geometry>
184
+ <material name="Blanc"/>
185
+ </visual>
186
+ <!-- <collision>
187
+ <geometry>
188
+ <mesh filename="robotiq-2f-driver.stl" scale="0.001 0.001 0.001"/>
189
+ </geometry>
190
+ </collision> -->
191
+ </link>
192
+ <joint name="robotiq_2f_85_left_coupler_joint" type="fixed">
193
+ <origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
194
+ <parent link="robotiq_2f_85_left_driver"/>
195
+ <child link="robotiq_2f_85_left_coupler"/>
196
+ </joint>
197
+ <link name="robotiq_2f_85_left_coupler">
198
+ <inertial>
199
+ <mass value="0.1"/>
200
+ <origin xyz="0 0 0.055"/>
201
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
202
+ </inertial>
203
+ <visual>
204
+ <geometry>
205
+ <mesh filename="robotiq-2f-coupler.obj" scale="0.1 0.1 0.1"/>
206
+ </geometry>
207
+ <material name="Blanc"/>
208
+ </visual>
209
+ <!-- <collision>
210
+ <geometry>
211
+ <mesh filename="robotiq-2f-coupler.stl" scale="0.001 0.001 0.001"/>
212
+ </geometry>
213
+ </collision> -->
214
+ </link>
215
+ <joint name="robotiq_2f_85_left_follower_joint" type="revolute">
216
+ <origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
217
+ <parent link="robotiq_2f_85_left_coupler"/>
218
+ <child link="robotiq_2f_85_left_follower"/>
219
+ <axis xyz="1 0 0"/>
220
+ <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
221
+ <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="-1"/>
222
+ </joint>
223
+ <link name="robotiq_2f_85_left_follower">
224
+ <visual>
225
+ <geometry>
226
+ <mesh filename="robotiq-2f-follower.obj" scale="0.1 0.1 0.1"/>
227
+ </geometry>
228
+ <material name="Blanc"/>
229
+ </visual>
230
+ <!-- <collision>
231
+ <geometry>
232
+ <mesh filename="robotiq-2f-follower.stl" scale="0.001 0.001 0.001"/>
233
+ </geometry>
234
+ </collision> -->
235
+ <inertial>
236
+ <mass value="0.1"/>
237
+ <origin xyz="0 0 0.055"/>
238
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
239
+ </inertial>
240
+ </link>
241
+ <joint name="robotiq_2f_85_left_pad_joint" type="fixed">
242
+ <parent link="robotiq_2f_85_left_follower"/>
243
+ <child link="robotiq_2f_85_left_pad"/>
244
+ </joint>
245
+ <link name="robotiq_2f_85_left_pad">
246
+ <inertial>
247
+ <mass value="0.1"/>
248
+ <origin xyz="0 0 0.055" />
249
+ <inertia ixx="0.00019" iyy="0.00018" izz="0.00019" ixy="0" iyz="0" ixz="0"/>
250
+ </inertial>
251
+ <visual>
252
+ <geometry>
253
+ <mesh filename="robotiq-2f-pad.stl" scale="0.0001 0.0001 0.0001"/>
254
+ </geometry>
255
+ <material name="Blanc"/>
256
+ </visual>
257
+ <collision>
258
+ <geometry>
259
+ <mesh filename="robotiq-2f-pad.stl" scale="0.001 0.001 0.001"/>
260
+ </geometry>
261
+ </collision>
262
+ </link>
263
+ <joint name="robotiq_2f_85_left_spring_link_joint" type="revolute">
264
+ <origin rpy="0 0 3.141592653589793" xyz="0 -0.012 0.0614"/>
265
+ <parent link="robotiq_2f_85_base"/>
266
+ <child link="robotiq_2f_85_left_spring_link"/>
267
+ <axis xyz="1 0 0"/>
268
+ <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
269
+ <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="1"/>
270
+ </joint>
271
+ <link name="robotiq_2f_85_left_spring_link">
272
+ <inertial>
273
+ <mass value="0.1"/>
274
+ <origin xyz="0 0 0.055"/>
275
+ <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
276
+ </inertial>
277
+ <visual>
278
+ <geometry>
279
+ <mesh filename="robotiq-2f-spring_link.obj" scale="0.1 0.1 0.1"/>
280
+ </geometry>
281
+ <material name="Blanc"/>
282
+ </visual>
283
+ <!-- <collision>
284
+ <geometry>
285
+ <mesh filename="robotiq-2f-spring_link.stl" scale="0.001 0.001 0.001"/>
286
+ </geometry>
287
+ </collision> -->
288
+ </link>
289
+ <transmission name="robotiq_2f_85_right_driver_trans">
290
+ <type>transmission_interface/SimpleTransmission</type>
291
+ <joint name="robotiq_2f_85_right_driver_joint">
292
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
293
+ </joint>
294
+ <actuator name="robotiq_2f_85_right_driver_motor">
295
+ <mechanicalReduction>1</mechanicalReduction>
296
+ </actuator>
297
+ </transmission>
298
+ </robot>
299
+
robotiq_2f_85/textures/gripper-2f_BaseColor.jpg ADDED
robotiq_2f_85/textures/gripper-2f_Metallic.jpg ADDED
robotiq_2f_85/textures/gripper-2f_Normal.jpg ADDED
robotiq_2f_85/textures/gripper-2f_Roughness.jpg ADDED
sim.py ADDED
@@ -0,0 +1,655 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import pybullet
2
+ import pybullet_data
3
+ import threading
4
+ from time import sleep
5
+ import numpy as np
6
+ import os
7
+ from consts import BOUNDS, COLORS, PIXEL_SIZE, CORNER_POS
8
+ from shapely.geometry import box
9
+
10
+ # Gripper (Robotiq 2F85) code
11
+ class Robotiq2F85:
12
+ """Gripper handling for Robotiq 2F85."""
13
+
14
+ def __init__(self, robot, tool):
15
+ self.robot = robot
16
+ self.tool = tool
17
+ pos = [0.1339999999999999, -0.49199999999872496, 0.5]
18
+ rot = pybullet.getQuaternionFromEuler([np.pi, 0, np.pi])
19
+ urdf = 'robotiq_2f_85/robotiq_2f_85.urdf'
20
+ self.body = pybullet.loadURDF(urdf, pos, rot)
21
+ self.n_joints = pybullet.getNumJoints(self.body)
22
+ self.activated = False
23
+
24
+ # Connect gripper base to robot tool.
25
+ 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]))
26
+
27
+ # Set friction coefficients for gripper fingers.
28
+ for i in range(pybullet.getNumJoints(self.body)):
29
+ pybullet.changeDynamics(self.body, i, lateralFriction=10.0, spinningFriction=1.0, rollingFriction=1.0, frictionAnchor=True)
30
+
31
+ # Start thread to handle additional gripper constraints.
32
+ self.motor_joint = 1
33
+ self.constraints_thread = threading.Thread(target=self.step)
34
+ self.constraints_thread.daemon = True
35
+ self.constraints_thread.start()
36
+
37
+ # Control joint positions by enforcing hard contraints on gripper behavior.
38
+ # Set one joint as the open/close motor joint (other joints should mimic).
39
+ def step(self):
40
+ while True:
41
+ try:
42
+ currj = [pybullet.getJointState(self.body, i)[0] for i in range(self.n_joints)]
43
+ indj = [6, 3, 8, 5, 10]
44
+ targj = [currj[1], -currj[1], -currj[1], currj[1], currj[1]]
45
+ pybullet.setJointMotorControlArray(self.body, indj, pybullet.POSITION_CONTROL, targj, positionGains=np.ones(5))
46
+ except:
47
+ return
48
+ sleep(0.001)
49
+
50
+ # Close gripper fingers.
51
+ def activate(self):
52
+ pybullet.setJointMotorControl2(self.body, self.motor_joint, pybullet.VELOCITY_CONTROL, targetVelocity=1, force=10)
53
+ self.activated = True
54
+
55
+ # Open gripper fingers.
56
+ def release(self):
57
+ pybullet.setJointMotorControl2(self.body, self.motor_joint, pybullet.VELOCITY_CONTROL, targetVelocity=-1, force=10)
58
+ self.activated = False
59
+
60
+ # If activated and object in gripper: check object contact.
61
+ # If activated and nothing in gripper: check gripper contact.
62
+ # If released: check proximity to surface (disabled).
63
+ def detect_contact(self):
64
+ obj, _, ray_frac = self.check_proximity()
65
+ if self.activated:
66
+ empty = self.grasp_width() < 0.01
67
+ cbody = self.body if empty else obj
68
+ if obj == self.body or obj == 0:
69
+ return False
70
+ return self.external_contact(cbody)
71
+ # else:
72
+ # return ray_frac < 0.14 or self.external_contact()
73
+
74
+ # Return if body is in contact with something other than gripper
75
+ def external_contact(self, body=None):
76
+ if body is None:
77
+ body = self.body
78
+ pts = pybullet.getContactPoints(bodyA=body)
79
+ pts = [pt for pt in pts if pt[2] != self.body]
80
+ return len(pts) > 0 # pylint: disable=g-explicit-length-test
81
+
82
+ def check_grasp(self):
83
+ while self.moving():
84
+ sleep(0.001)
85
+ success = self.grasp_width() > 0.01
86
+ return success
87
+
88
+ def grasp_width(self):
89
+ lpad = np.array(pybullet.getLinkState(self.body, 4)[0])
90
+ rpad = np.array(pybullet.getLinkState(self.body, 9)[0])
91
+ dist = np.linalg.norm(lpad - rpad) - 0.047813
92
+ return dist
93
+
94
+ def check_proximity(self):
95
+ ee_pos = np.array(pybullet.getLinkState(self.robot, self.tool)[0])
96
+ tool_pos = np.array(pybullet.getLinkState(self.body, 0)[0])
97
+ vec = (tool_pos - ee_pos) / np.linalg.norm((tool_pos - ee_pos))
98
+ ee_targ = ee_pos + vec
99
+ ray_data = pybullet.rayTest(ee_pos, ee_targ)[0]
100
+ obj, link, ray_frac = ray_data[0], ray_data[1], ray_data[2]
101
+ return obj, link, ray_frac
102
+
103
+
104
+ # Gym-style environment code
105
+ class PickPlaceEnv():
106
+
107
+ def __init__(self, render=False, high_res=False, high_frame_rate=False):
108
+ self.dt = 1/480
109
+ self.sim_step = 0
110
+
111
+ # Configure and start PyBullet.
112
+ # python3 -m pybullet_utils.runServer
113
+ # pybullet.connect(pybullet.SHARED_MEMORY) # pybullet.GUI for local GUI.
114
+ pybullet.connect(pybullet.DIRECT) # pybullet.GUI for local GUI.
115
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
116
+ pybullet.setPhysicsEngineParameter(enableFileCaching=0)
117
+ assets_path = os.path.dirname(os.path.abspath(""))
118
+ pybullet.setAdditionalSearchPath(assets_path)
119
+ pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
120
+ pybullet.setTimeStep(self.dt)
121
+
122
+ 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).
123
+ self.home_ee_euler = (np.pi, 0, np.pi) # (RX, RY, RZ) rotation in Euler angles.
124
+ self.ee_link_id = 9 # Link ID of UR5 end effector.
125
+ self.tip_link_id = 10 # Link ID of gripper finger tips.
126
+ self.gripper = None
127
+
128
+ self.render = render
129
+ self.high_res = high_res
130
+ self.high_frame_rate = high_frame_rate
131
+
132
+ def reset(self, object_list):
133
+ pybullet.resetSimulation(pybullet.RESET_USE_DEFORMABLE_WORLD)
134
+ pybullet.setGravity(0, 0, -9.8)
135
+ self.cache_video = []
136
+
137
+ # Temporarily disable rendering to load URDFs faster.
138
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
139
+
140
+ # Add robot.
141
+ pybullet.loadURDF("plane.urdf", [0, 0, -0.001])
142
+ self.robot_id = pybullet.loadURDF("ur5e/ur5e.urdf", [0, 0, 0], flags=pybullet.URDF_USE_MATERIAL_COLORS_FROM_MTL)
143
+ self.ghost_id = pybullet.loadURDF("ur5e/ur5e.urdf", [0, 0, -10]) # For forward kinematics.
144
+ self.joint_ids = [pybullet.getJointInfo(self.robot_id, i) for i in range(pybullet.getNumJoints(self.robot_id))]
145
+ self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pybullet.JOINT_REVOLUTE]
146
+
147
+ # Move robot to home configuration.
148
+ for i in range(len(self.joint_ids)):
149
+ pybullet.resetJointState(self.robot_id, self.joint_ids[i], self.home_joints[i])
150
+
151
+ # Add gripper.
152
+ if self.gripper is not None:
153
+ while self.gripper.constraints_thread.is_alive():
154
+ self.constraints_thread_active = False
155
+ self.gripper = Robotiq2F85(self.robot_id, self.ee_link_id)
156
+ self.gripper.release()
157
+
158
+ # Add workspace.
159
+ plane_shape = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=[0.3, 0.3, 0.001])
160
+ plane_visual = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=[0.3, 0.3, 0.001])
161
+ plane_id = pybullet.createMultiBody(0, plane_shape, plane_visual, basePosition=[0, -0.5, 0])
162
+ pybullet.changeVisualShape(plane_id, -1, rgbaColor=[0.2, 0.2, 0.2, 1.0])
163
+
164
+ # Load objects according to config.
165
+ self.object_list = object_list
166
+ self.obj_name_to_id = {}
167
+ obj_xyz = np.zeros((0, 3))
168
+ for obj_name in object_list:
169
+ if ('block' in obj_name) or ('bowl' in obj_name):
170
+
171
+ # Get random position 15cm+ from other objects.
172
+ while True:
173
+ rand_x = np.random.uniform(BOUNDS[0, 0] + 0.1, BOUNDS[0, 1] - 0.1)
174
+ rand_y = np.random.uniform(BOUNDS[1, 0] + 0.1, BOUNDS[1, 1] - 0.1)
175
+ rand_xyz = np.float32([rand_x, rand_y, 0.03]).reshape(1, 3)
176
+ if len(obj_xyz) == 0:
177
+ obj_xyz = np.concatenate((obj_xyz, rand_xyz), axis=0)
178
+ break
179
+ else:
180
+ nn_dist = np.min(np.linalg.norm(obj_xyz - rand_xyz, axis=1)).squeeze()
181
+ if nn_dist > 0.15:
182
+ obj_xyz = np.concatenate((obj_xyz, rand_xyz), axis=0)
183
+ break
184
+
185
+ object_color = COLORS[obj_name.split(' ')[0]]
186
+ object_type = obj_name.split(' ')[1]
187
+ object_position = rand_xyz.squeeze()
188
+ if object_type == 'block':
189
+ object_shape = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=[0.02, 0.02, 0.02])
190
+ object_visual = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=[0.02, 0.02, 0.02])
191
+ object_id = pybullet.createMultiBody(0.01, object_shape, object_visual, basePosition=object_position)
192
+ elif object_type == 'bowl':
193
+ object_position[2] = 0
194
+ object_id = pybullet.loadURDF("bowl/bowl.urdf", object_position, useFixedBase=1)
195
+ pybullet.changeVisualShape(object_id, -1, rgbaColor=object_color)
196
+ self.obj_name_to_id[obj_name] = object_id
197
+
198
+
199
+ # Re-enable rendering.
200
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
201
+
202
+ for _ in range(200):
203
+ pybullet.stepSimulation()
204
+
205
+ # record object positions at reset
206
+ self.init_pos = {name: self.get_obj_pos(name) for name in object_list}
207
+
208
+ return self.get_observation()
209
+
210
+ def servoj(self, joints):
211
+ """Move to target joint positions with position control."""
212
+ pybullet.setJointMotorControlArray(
213
+ bodyIndex=self.robot_id,
214
+ jointIndices=self.joint_ids,
215
+ controlMode=pybullet.POSITION_CONTROL,
216
+ targetPositions=joints,
217
+ positionGains=[0.01]*6)
218
+
219
+ def movep(self, position):
220
+ """Move to target end effector position."""
221
+ joints = pybullet.calculateInverseKinematics(
222
+ bodyUniqueId=self.robot_id,
223
+ endEffectorLinkIndex=self.tip_link_id,
224
+ targetPosition=position,
225
+ targetOrientation=pybullet.getQuaternionFromEuler(self.home_ee_euler),
226
+ maxNumIterations=100)
227
+ self.servoj(joints)
228
+
229
+ def get_ee_pos(self):
230
+ ee_xyz = np.float32(pybullet.getLinkState(self.robot_id, self.tip_link_id)[0])
231
+ return ee_xyz
232
+
233
+ def step(self, action=None):
234
+ """Do pick and place motion primitive."""
235
+ pick_pos, place_pos = action['pick'].copy(), action['place'].copy()
236
+
237
+ # Set fixed primitive z-heights.
238
+ hover_xyz = np.float32([pick_pos[0], pick_pos[1], 0.2])
239
+ if pick_pos.shape[-1] == 2:
240
+ pick_xyz = np.append(pick_pos, 0.025)
241
+ else:
242
+ pick_xyz = pick_pos
243
+ pick_xyz[2] = 0.025
244
+ if place_pos.shape[-1] == 2:
245
+ place_xyz = np.append(place_pos, 0.15)
246
+ else:
247
+ place_xyz = place_pos
248
+ place_xyz[2] = 0.15
249
+
250
+ # Move to object.
251
+ ee_xyz = self.get_ee_pos()
252
+ while np.linalg.norm(hover_xyz - ee_xyz) > 0.01:
253
+ self.movep(hover_xyz)
254
+ self.step_sim_and_render()
255
+ ee_xyz = self.get_ee_pos()
256
+
257
+ while np.linalg.norm(pick_xyz - ee_xyz) > 0.01:
258
+ self.movep(pick_xyz)
259
+ self.step_sim_and_render()
260
+ ee_xyz = self.get_ee_pos()
261
+
262
+ # Pick up object.
263
+ self.gripper.activate()
264
+ for _ in range(240):
265
+ self.step_sim_and_render()
266
+ while np.linalg.norm(hover_xyz - ee_xyz) > 0.01:
267
+ self.movep(hover_xyz)
268
+ self.step_sim_and_render()
269
+ ee_xyz = self.get_ee_pos()
270
+
271
+ for _ in range(50):
272
+ self.step_sim_and_render()
273
+
274
+ # Move to place location.
275
+ while np.linalg.norm(place_xyz - ee_xyz) > 0.01:
276
+ self.movep(place_xyz)
277
+ self.step_sim_and_render()
278
+ ee_xyz = self.get_ee_pos()
279
+
280
+ # Place down object.
281
+ while (not self.gripper.detect_contact()) and (place_xyz[2] > 0.03):
282
+ place_xyz[2] -= 0.001
283
+ self.movep(place_xyz)
284
+ for _ in range(3):
285
+ self.step_sim_and_render()
286
+ self.gripper.release()
287
+ for _ in range(240):
288
+ self.step_sim_and_render()
289
+ place_xyz[2] = 0.2
290
+ ee_xyz = self.get_ee_pos()
291
+ while np.linalg.norm(place_xyz - ee_xyz) > 0.01:
292
+ self.movep(place_xyz)
293
+ self.step_sim_and_render()
294
+ ee_xyz = self.get_ee_pos()
295
+ place_xyz = np.float32([0, -0.5, 0.2])
296
+ while np.linalg.norm(place_xyz - ee_xyz) > 0.01:
297
+ self.movep(place_xyz)
298
+ self.step_sim_and_render()
299
+ ee_xyz = self.get_ee_pos()
300
+
301
+ observation = self.get_observation()
302
+ reward = self.get_reward()
303
+ done = False
304
+ info = {}
305
+ return observation, reward, done, info
306
+
307
+ def set_alpha_transparency(self, alpha: float) -> None:
308
+ for id in range(20):
309
+ visual_shape_data = pybullet.getVisualShapeData(id)
310
+ for i in range(len(visual_shape_data)):
311
+ object_id, link_index, _, _, _, _, _, rgba_color = visual_shape_data[i]
312
+ rgba_color = list(rgba_color[0:3]) + [alpha]
313
+ pybullet.changeVisualShape(
314
+ self.robot_id, linkIndex=i, rgbaColor=rgba_color)
315
+ pybullet.changeVisualShape(
316
+ self.gripper.body, linkIndex=i, rgbaColor=rgba_color)
317
+
318
+ def step_sim_and_render(self):
319
+ pybullet.stepSimulation()
320
+ self.sim_step += 1
321
+
322
+ interval = 40 if self.high_frame_rate else 60
323
+ # Render current image at 8 FPS.
324
+ if self.sim_step % interval == 0 and self.render:
325
+ self.cache_video.append(self.get_camera_image())
326
+
327
+ def get_camera_image(self):
328
+ if not self.high_res:
329
+ image_size = (240, 240)
330
+ intrinsics = (120., 0, 120., 0, 120., 120., 0, 0, 1)
331
+ else:
332
+ image_size=(360, 360)
333
+ intrinsics=(180., 0, 180., 0, 180., 180., 0, 0, 1)
334
+ color, _, _, _, _ = self.render_image(image_size, intrinsics)
335
+ return color
336
+
337
+ def get_reward(self):
338
+ return None
339
+
340
+ def get_observation(self):
341
+ observation = {}
342
+
343
+ # Render current image.
344
+ color, depth, position, orientation, intrinsics = self.render_image()
345
+
346
+ # Get heightmaps and colormaps.
347
+ points = self.get_pointcloud(depth, intrinsics)
348
+ position = np.float32(position).reshape(3, 1)
349
+ rotation = pybullet.getMatrixFromQuaternion(orientation)
350
+ rotation = np.float32(rotation).reshape(3, 3)
351
+ transform = np.eye(4)
352
+ transform[:3, :] = np.hstack((rotation, position))
353
+ points = self.transform_pointcloud(points, transform)
354
+ heightmap, colormap, xyzmap = self.get_heightmap(points, color, BOUNDS, PIXEL_SIZE)
355
+
356
+ observation["image"] = colormap
357
+ observation["xyzmap"] = xyzmap
358
+
359
+ return observation
360
+
361
+ def render_image(self, image_size=(720, 720), intrinsics=(360., 0, 360., 0, 360., 360., 0, 0, 1)):
362
+
363
+ # Camera parameters.
364
+ position = (0, -0.85, 0.4)
365
+ orientation = (np.pi / 4 + np.pi / 48, np.pi, np.pi)
366
+ orientation = pybullet.getQuaternionFromEuler(orientation)
367
+ zrange = (0.01, 10.)
368
+ noise=True
369
+
370
+ # OpenGL camera settings.
371
+ lookdir = np.float32([0, 0, 1]).reshape(3, 1)
372
+ updir = np.float32([0, -1, 0]).reshape(3, 1)
373
+ rotation = pybullet.getMatrixFromQuaternion(orientation)
374
+ rotm = np.float32(rotation).reshape(3, 3)
375
+ lookdir = (rotm @ lookdir).reshape(-1)
376
+ updir = (rotm @ updir).reshape(-1)
377
+ lookat = position + lookdir
378
+ focal_len = intrinsics[0]
379
+ znear, zfar = (0.01, 10.)
380
+ viewm = pybullet.computeViewMatrix(position, lookat, updir)
381
+ fovh = (image_size[0] / 2) / focal_len
382
+ fovh = 180 * np.arctan(fovh) * 2 / np.pi
383
+
384
+ # Notes: 1) FOV is vertical FOV 2) aspect must be float
385
+ aspect_ratio = image_size[1] / image_size[0]
386
+ projm = pybullet.computeProjectionMatrixFOV(fovh, aspect_ratio, znear, zfar)
387
+
388
+ # Render with OpenGL camera settings.
389
+ _, _, color, depth, segm = pybullet.getCameraImage(
390
+ width=image_size[1],
391
+ height=image_size[0],
392
+ viewMatrix=viewm,
393
+ projectionMatrix=projm,
394
+ shadow=1,
395
+ flags=pybullet.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
396
+ renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
397
+
398
+ # Get color image.
399
+ color_image_size = (image_size[0], image_size[1], 4)
400
+ color = np.array(color, dtype=np.uint8).reshape(color_image_size)
401
+ color = color[:, :, :3] # remove alpha channel
402
+ if noise:
403
+ color = np.int32(color)
404
+ color += np.int32(np.random.normal(0, 3, color.shape))
405
+ color = np.uint8(np.clip(color, 0, 255))
406
+
407
+ # Get depth image.
408
+ depth_image_size = (image_size[0], image_size[1])
409
+ zbuffer = np.float32(depth).reshape(depth_image_size)
410
+ depth = (zfar + znear - (2 * zbuffer - 1) * (zfar - znear))
411
+ depth = (2 * znear * zfar) / depth
412
+ if noise:
413
+ depth += np.random.normal(0, 0.003, depth.shape)
414
+
415
+ intrinsics = np.float32(intrinsics).reshape(3, 3)
416
+ return color, depth, position, orientation, intrinsics
417
+
418
+ def get_pointcloud(self, depth, intrinsics):
419
+ """Get 3D pointcloud from perspective depth image.
420
+ Args:
421
+ depth: HxW float array of perspective depth in meters.
422
+ intrinsics: 3x3 float array of camera intrinsics matrix.
423
+ Returns:
424
+ points: HxWx3 float array of 3D points in camera coordinates.
425
+ """
426
+ height, width = depth.shape
427
+ xlin = np.linspace(0, width - 1, width)
428
+ ylin = np.linspace(0, height - 1, height)
429
+ px, py = np.meshgrid(xlin, ylin)
430
+ px = (px - intrinsics[0, 2]) * (depth / intrinsics[0, 0])
431
+ py = (py - intrinsics[1, 2]) * (depth / intrinsics[1, 1])
432
+ points = np.float32([px, py, depth]).transpose(1, 2, 0)
433
+ return points
434
+
435
+ def transform_pointcloud(self, points, transform):
436
+ """Apply rigid transformation to 3D pointcloud.
437
+ Args:
438
+ points: HxWx3 float array of 3D points in camera coordinates.
439
+ transform: 4x4 float array representing a rigid transformation matrix.
440
+ Returns:
441
+ points: HxWx3 float array of transformed 3D points.
442
+ """
443
+ padding = ((0, 0), (0, 0), (0, 1))
444
+ homogen_points = np.pad(points.copy(), padding,
445
+ 'constant', constant_values=1)
446
+ for i in range(3):
447
+ points[Ellipsis, i] = np.sum(transform[i, :] * homogen_points, axis=-1)
448
+ return points
449
+
450
+ def get_heightmap(self, points, colors, bounds, pixel_size):
451
+ """Get top-down (z-axis) orthographic heightmap image from 3D pointcloud.
452
+ Args:
453
+ points: HxWx3 float array of 3D points in world coordinates.
454
+ colors: HxWx3 uint8 array of values in range 0-255 aligned with points.
455
+ bounds: 3x2 float array of values (rows: X,Y,Z; columns: min,max) defining
456
+ region in 3D space to generate heightmap in world coordinates.
457
+ pixel_size: float defining size of each pixel in meters.
458
+ Returns:
459
+ heightmap: HxW float array of height (from lower z-bound) in meters.
460
+ colormap: HxWx3 uint8 array of backprojected color aligned with heightmap.
461
+ xyzmap: HxWx3 float array of XYZ points in world coordinates.
462
+ """
463
+ width = int(np.round((bounds[0, 1] - bounds[0, 0]) / pixel_size))
464
+ height = int(np.round((bounds[1, 1] - bounds[1, 0]) / pixel_size))
465
+ heightmap = np.zeros((height, width), dtype=np.float32)
466
+ colormap = np.zeros((height, width, colors.shape[-1]), dtype=np.uint8)
467
+ xyzmap = np.zeros((height, width, 3), dtype=np.float32)
468
+
469
+ # Filter out 3D points that are outside of the predefined bounds.
470
+ ix = (points[Ellipsis, 0] >= bounds[0, 0]) & (points[Ellipsis, 0] < bounds[0, 1])
471
+ iy = (points[Ellipsis, 1] >= bounds[1, 0]) & (points[Ellipsis, 1] < bounds[1, 1])
472
+ iz = (points[Ellipsis, 2] >= bounds[2, 0]) & (points[Ellipsis, 2] < bounds[2, 1])
473
+ valid = ix & iy & iz
474
+ points = points[valid]
475
+ colors = colors[valid]
476
+
477
+ # Sort 3D points by z-value, which works with array assignment to simulate
478
+ # z-buffering for rendering the heightmap image.
479
+ iz = np.argsort(points[:, -1])
480
+ points, colors = points[iz], colors[iz]
481
+ px = np.int32(np.floor((points[:, 0] - bounds[0, 0]) / pixel_size))
482
+ py = np.int32(np.floor((points[:, 1] - bounds[1, 0]) / pixel_size))
483
+ px = np.clip(px, 0, width - 1)
484
+ py = np.clip(py, 0, height - 1)
485
+ heightmap[py, px] = points[:, 2] - bounds[2, 0]
486
+ for c in range(colors.shape[-1]):
487
+ colormap[py, px, c] = colors[:, c]
488
+ xyzmap[py, px, c] = points[:, c]
489
+ colormap = colormap[::-1, :, :] # Flip up-down.
490
+ xv, yv = np.meshgrid(np.linspace(BOUNDS[0, 0], BOUNDS[0, 1], height),
491
+ np.linspace(BOUNDS[1, 0], BOUNDS[1, 1], width))
492
+ xyzmap[:, :, 0] = xv
493
+ xyzmap[:, :, 1] = yv
494
+ xyzmap = xyzmap[::-1, :, :] # Flip up-down.
495
+ heightmap = heightmap[::-1, :] # Flip up-down.
496
+ return heightmap, colormap, xyzmap
497
+
498
+ def on_top_of(self, obj_a, obj_b):
499
+ """
500
+ check if obj_a is on top of obj_b
501
+ condition 1: l2 distance on xy plane is less than a threshold
502
+ condition 2: obj_a is higher than obj_b
503
+ """
504
+ obj_a_pos = self.get_obj_pos(obj_a)
505
+ obj_b_pos = self.get_obj_pos(obj_b)
506
+ xy_dist = np.linalg.norm(obj_a_pos[:2] - obj_b_pos[:2])
507
+ if obj_b in CORNER_POS:
508
+ is_near = xy_dist < 0.06
509
+ return is_near
510
+ elif 'bowl' in obj_b:
511
+ is_near = xy_dist < 0.06
512
+ is_higher = obj_a_pos[2] > obj_b_pos[2]
513
+ return is_near and is_higher
514
+ else:
515
+ is_near = xy_dist < 0.04
516
+ is_higher = obj_a_pos[2] > obj_b_pos[2]
517
+ return is_near and is_higher
518
+
519
+ def get_obj_id(self, obj_name):
520
+ try:
521
+ if obj_name in self.obj_name_to_id:
522
+ obj_id = self.obj_name_to_id[obj_name]
523
+ else:
524
+ obj_name = obj_name.replace('circle', 'bowl').replace('square', 'block').replace('small', '').strip()
525
+ obj_id = self.obj_name_to_id[obj_name]
526
+ except:
527
+ print(f'requested_name="{obj_name}"')
528
+ print(f'available_objects_and_id="{self.obj_name_to_id}')
529
+ return obj_id
530
+
531
+ def get_obj_pos(self, obj_name):
532
+ obj_name = obj_name.replace('the', '').replace('_', ' ').strip()
533
+ if obj_name in CORNER_POS:
534
+ position = np.float32(np.array(CORNER_POS[obj_name]))
535
+ else:
536
+ pick_id = self.get_obj_id(obj_name)
537
+ pose = pybullet.getBasePositionAndOrientation(pick_id)
538
+ position = np.float32(pose[0])
539
+ return position
540
+
541
+ def get_bounding_box(self, obj_name):
542
+ obj_id = self.get_obj_id(obj_name)
543
+ return pybullet.getAABB(obj_id)
544
+
545
+
546
+ class LMP_wrapper():
547
+
548
+ def __init__(self, env, cfg, render=False):
549
+ self.env = env
550
+ self._cfg = cfg
551
+ self.object_names = list(self._cfg['env']['init_objs'])
552
+
553
+ self._min_xy = np.array(self._cfg['env']['coords']['bottom_left'])
554
+ self._max_xy = np.array(self._cfg['env']['coords']['top_right'])
555
+ self._range_xy = self._max_xy - self._min_xy
556
+
557
+ self._table_z = self._cfg['env']['coords']['table_z']
558
+ self.render = render
559
+
560
+ def is_obj_visible(self, obj_name):
561
+ return obj_name in self.object_names
562
+
563
+ def get_obj_names(self):
564
+ return self.object_names[::]
565
+
566
+ def denormalize_xy(self, pos_normalized):
567
+ return pos_normalized * self._range_xy + self._min_xy
568
+
569
+ def get_corner_positions(self):
570
+ unit_square = box(0, 0, 1, 1)
571
+ normalized_corners = np.array(list(unit_square.exterior.coords))[:4]
572
+ corners = np.array(([self.denormalize_xy(corner) for corner in normalized_corners]))
573
+ return corners
574
+
575
+ def get_side_positions(self):
576
+ side_xs = np.array([0, 0.5, 0.5, 1])
577
+ side_ys = np.array([0.5, 0, 1, 0.5])
578
+ normalized_side_positions = np.c_[side_xs, side_ys]
579
+ side_positions = np.array(([self.denormalize_xy(corner) for corner in normalized_side_positions]))
580
+ return side_positions
581
+
582
+ def get_obj_pos(self, obj_name):
583
+ # return the xy position of the object in robot base frame
584
+ return self.env.get_obj_pos(obj_name)[:2]
585
+
586
+ def get_obj_position_np(self, obj_name):
587
+ return self.get_pos(obj_name)
588
+
589
+ def get_bbox(self, obj_name):
590
+ # return the axis-aligned object bounding box in robot base frame (not in pixels)
591
+ # the format is (min_x, min_y, max_x, max_y)
592
+ bbox = self.env.get_bounding_box(obj_name)
593
+ return bbox
594
+
595
+ def get_color(self, obj_name):
596
+ for color, rgb in COLORS.items():
597
+ if color in obj_name:
598
+ return rgb
599
+
600
+ def pick_place(self, pick_pos, place_pos):
601
+ pick_pos_xyz = np.r_[pick_pos, [self._table_z]]
602
+ place_pos_xyz = np.r_[place_pos, [self._table_z]]
603
+ pass
604
+
605
+ def put_first_on_second(self, arg1, arg2):
606
+ # put the object with obj_name on top of target
607
+ # target can either be another object name, or it can be an x-y position in robot base frame
608
+ pick_pos = self.get_obj_pos(arg1) if isinstance(arg1, str) else arg1
609
+ place_pos = self.get_obj_pos(arg2) if isinstance(arg2, str) else arg2
610
+ self.env.step(action={'pick': pick_pos, 'place': place_pos})
611
+
612
+ def get_robot_pos(self):
613
+ # return robot end-effector xy position in robot base frame
614
+ return self.env.get_ee_pos()
615
+
616
+ def goto_pos(self, position_xy):
617
+ # move the robot end-effector to the desired xy position while maintaining same z
618
+ ee_xyz = self.env.get_ee_pos()
619
+ position_xyz = np.concatenate([position_xy, ee_xyz[-1]])
620
+ while np.linalg.norm(position_xyz - ee_xyz) > 0.01:
621
+ self.env.movep(position_xyz)
622
+ self.env.step_sim_and_render()
623
+ ee_xyz = self.env.get_ee_pos()
624
+
625
+ def follow_traj(self, traj):
626
+ for pos in traj:
627
+ self.goto_pos(pos)
628
+
629
+ def get_corner_positions(self):
630
+ normalized_corners = np.array([
631
+ [0, 1],
632
+ [1, 1],
633
+ [0, 0],
634
+ [1, 0]
635
+ ])
636
+ return np.array(([self.denormalize_xy(corner) for corner in normalized_corners]))
637
+
638
+ def get_side_positions(self):
639
+ normalized_sides = np.array([
640
+ [0.5, 1],
641
+ [1, 0.5],
642
+ [0.5, 0],
643
+ [0, 0.5]
644
+ ])
645
+ return np.array(([self.denormalize_xy(side) for side in normalized_sides]))
646
+
647
+ def get_corner_name(self, pos):
648
+ corner_positions = self.get_corner_positions()
649
+ corner_idx = np.argmin(np.linalg.norm(corner_positions - pos, axis=1))
650
+ return ['top left corner', 'top right corner', 'bottom left corner', 'botom right corner'][corner_idx]
651
+
652
+ def get_side_name(self, pos):
653
+ side_positions = self.get_side_positions()
654
+ side_idx = np.argmin(np.linalg.norm(side_positions - pos, axis=1))
655
+ return ['top side', 'right side', 'bottom side', 'left side'][side_idx]
ur5e/collision/base.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:9a145e0d46f2130afdf2a2e8825a00a929870c4c3d6d8e4d1adc5f04db3aac1b
3
+ size 21084
ur5e/collision/forearm.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:2e7423cab807c34160ec4f770daee5e747d70e777eb01b7beeace2b8c5751816
3
+ size 53284
ur5e/collision/shoulder.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:ceb92532177daa77682f5fbd628e01c2137d168f949a7a706ce1dabe9f002387
3
+ size 70084
ur5e/collision/upperarm.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:ee893044caf00075cb55b4cf666d1f1311c7979786212a501009f33bee945209
3
+ size 99684
ur5e/collision/wrist1.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:f8c9f9337b6fd98c75f052e96de10e14a107ddb6874ba6b904e546f8a4e4f43a
3
+ size 59584
ur5e/collision/wrist2.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:2964a63f60ce3e3cf3ad55bcf190d7876d50e373cb64b70a57cea5885eaf3c86
3
+ size 67584
ur5e/collision/wrist3.stl ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ version https://git-lfs.github.com/spec/v1
2
+ oid sha256:83b3666b4ae2badd54af0d2c25a921682ecbc29e849eec646c3ed55fb74c78a3
3
+ size 7184
ur5e/ur5e.urdf ADDED
@@ -0,0 +1,279 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ <?xml version="1.0" ?>
2
+ <!-- ============================================================================================= -->
3
+ <!-- | This document was autogenerated by xacro from ur_description/urdf/ur5_robot.urdf.xacro | -->
4
+ <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
5
+ <!-- ============================================================================================= -->
6
+ <robot name="ur5" xmlns:xacro="http://ros.org/wiki/xacro">
7
+ <material name="LightGrey">
8
+ <color rgba="0.8 0.8 0.8 1.0"/>
9
+ </material>
10
+
11
+ <material name="DarkGrey">
12
+ <color rgba="0.2 0.2 0.2 1.0"/>
13
+ </material>
14
+
15
+ <link name="base_link">
16
+ <visual>
17
+ <geometry>
18
+ <mesh filename="visual/base.dae"/>
19
+ </geometry>
20
+ <material name="LightGrey"/>
21
+ </visual>
22
+ <!-- <collision>
23
+ <geometry>
24
+ <mesh filename="collision/base.stl"/>
25
+ </geometry>
26
+ </collision> -->
27
+ <inertial>
28
+ <mass value="4.0"/>
29
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
30
+ <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
31
+ </inertial>
32
+ </link>
33
+ <joint name="shoulder_pan_joint" type="revolute">
34
+ <parent link="base_link"/>
35
+ <child link="shoulder_link"/>
36
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.163"/>
37
+ <axis xyz="0 0 1"/>
38
+ <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
39
+ <dynamics damping="0.0" friction="0.0"/>
40
+ </joint>
41
+ <link name="shoulder_link">
42
+ <visual>
43
+ <geometry>
44
+ <mesh filename="visual/shoulder.dae"/>
45
+ </geometry>
46
+ <material name="DarkGrey"/>
47
+ </visual>
48
+ <!-- <collision>
49
+ <geometry>
50
+ <mesh filename="collision/shoulder.stl"/>
51
+ </geometry>
52
+ </collision> -->
53
+ <inertial>
54
+ <mass value="3.7"/>
55
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
56
+ <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
57
+ </inertial>
58
+ </link>
59
+ <joint name="shoulder_lift_joint" type="revolute">
60
+ <parent link="shoulder_link"/>
61
+ <child link="upper_arm_link"/>
62
+ <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.138 0.0"/>
63
+ <axis xyz="0 1 0"/>
64
+ <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
65
+ <dynamics damping="0.0" friction="0.0"/>
66
+ </joint>
67
+ <link name="upper_arm_link">
68
+ <visual>
69
+ <geometry>
70
+ <mesh filename="visual/upperarm.dae"/>
71
+ </geometry>
72
+ <material name="LightGrey"/>
73
+ </visual>
74
+ <!-- <collision>
75
+ <geometry>
76
+ <mesh filename="collision/upperarm.stl"/>
77
+ </geometry>
78
+ </collision> -->
79
+ <inertial>
80
+ <mass value="8.393"/>
81
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
82
+ <inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
83
+ </inertial>
84
+ </link>
85
+ <joint name="elbow_joint" type="revolute">
86
+ <parent link="upper_arm_link"/>
87
+ <child link="forearm_link"/>
88
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.131 0.425"/>
89
+ <axis xyz="0 1 0"/>
90
+ <limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
91
+ <dynamics damping="0.0" friction="0.0"/>
92
+ </joint>
93
+ <link name="forearm_link">
94
+ <visual>
95
+ <geometry>
96
+ <mesh filename="visual/forearm.dae"/>
97
+ </geometry>
98
+ <material name="DarkGrey"/>
99
+ </visual>
100
+ <!-- <collision>
101
+ <geometry>
102
+ <mesh filename="collision/forearm.stl"/>
103
+ </geometry>
104
+ </collision> -->
105
+ <inertial>
106
+ <mass value="2.275"/>
107
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
108
+ <inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
109
+ </inertial>
110
+ </link>
111
+ <joint name="wrist_1_joint" type="revolute">
112
+ <parent link="forearm_link"/>
113
+ <child link="wrist_1_link"/>
114
+ <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.392"/>
115
+ <axis xyz="0 1 0"/>
116
+ <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
117
+ <dynamics damping="0.0" friction="0.0"/>
118
+ </joint>
119
+ <link name="wrist_1_link">
120
+ <visual>
121
+ <geometry>
122
+ <mesh filename="visual/wrist1.dae"/>
123
+ </geometry>
124
+ <material name="LightGrey"/>
125
+ </visual>
126
+ <!-- <collision>
127
+ <geometry>
128
+ <mesh filename="collision/wrist1.stl"/>
129
+ </geometry>
130
+ </collision> -->
131
+ <inertial>
132
+ <mass value="1.219"/>
133
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
134
+ <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
135
+ </inertial>
136
+ </link>
137
+ <joint name="wrist_2_joint" type="revolute">
138
+ <parent link="wrist_1_link"/>
139
+ <child link="wrist_2_link"/>
140
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.127 0.0"/>
141
+ <axis xyz="0 0 1"/>
142
+ <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
143
+ <dynamics damping="0.0" friction="0.0"/>
144
+ </joint>
145
+ <link name="wrist_2_link">
146
+ <visual>
147
+ <geometry>
148
+ <mesh filename="visual/wrist2.dae"/>
149
+ </geometry>
150
+ <material name="DarkGrey"/>
151
+ </visual>
152
+ <collision>
153
+ <geometry>
154
+ <mesh filename="collision/wrist2.stl"/>
155
+ </geometry>
156
+ </collision>
157
+ <inertial>
158
+ <mass value="1.219"/>
159
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
160
+ <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
161
+ </inertial>
162
+ </link>
163
+ <joint name="wrist_3_joint" type="revolute">
164
+ <parent link="wrist_2_link"/>
165
+ <child link="wrist_3_link"/>
166
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.100"/>
167
+ <axis xyz="0 1 0"/>
168
+ <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
169
+ <dynamics damping="0.0" friction="0.0"/>
170
+ </joint>
171
+ <link name="wrist_3_link">
172
+ <visual>
173
+ <geometry>
174
+ <mesh filename="visual/wrist3.dae"/>
175
+ </geometry>
176
+ <material name="LightGrey"/>
177
+ </visual>
178
+ <collision>
179
+ <geometry>
180
+ <mesh filename="collision/wrist3.stl"/>
181
+ </geometry>
182
+ </collision>
183
+ <inertial>
184
+ <mass value="0.1879"/>
185
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
186
+ <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
187
+ </inertial>
188
+ </link>
189
+ <joint name="ee_fixed_joint" type="fixed">
190
+ <parent link="wrist_3_link"/>
191
+ <child link="ee_link"/>
192
+ <origin rpy="-1.5707963267948966 0.0 0.0" xyz="0.0 0.100 0.0"/>
193
+ </joint>
194
+ <link name="ee_link">
195
+ <!-- <collision>
196
+ <geometry>
197
+ <box size="0.01 0.01 0.01"/>
198
+ </geometry>
199
+ <origin rpy="0 0 0" xyz="-0.01 0 0"/>
200
+ </collision> -->
201
+ <inertial>
202
+ <mass value="0.0"/>
203
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
204
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
205
+ </inertial>
206
+ </link>
207
+ <!-- nothing to do here at the moment -->
208
+ <!-- ROS base_link to UR 'Base' Coordinates transform -->
209
+ <link name="base">
210
+ <inertial>
211
+ <mass value="0.0"/>
212
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
213
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
214
+ </inertial>
215
+ </link>
216
+ <joint name="base_link-base_fixed_joint" type="fixed">
217
+ <!-- NOTE: this rotation is only needed as long as base_link itself is
218
+ not corrected wrt the real robot (ie: rotated over 180
219
+ degrees)
220
+ -->
221
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
222
+ <parent link="base_link"/>
223
+ <child link="base"/>
224
+ </joint>
225
+ <!-- Frame coincident with all-zeros TCP on UR controller -->
226
+ <link name="tool0">
227
+ <inertial>
228
+ <mass value="0.0"/>
229
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
230
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
231
+ </inertial>
232
+ </link>
233
+ <joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
234
+ <origin rpy="-1.57079632679 0 0" xyz="0 0.0823 0"/>
235
+ <parent link="wrist_3_link"/>
236
+ <child link="tool0"/>
237
+ </joint>
238
+
239
+ <link name="tool_tip">
240
+ <inertial>
241
+ <mass value="0.0"/>
242
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
243
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
244
+ </inertial>
245
+ <!-- <visual>
246
+ <geometry>
247
+ <box size="0.01 0.01 0.01"/>
248
+ </geometry>
249
+ <material name="DarkGrey"/>
250
+ <origin rpy="0 0 0" xyz="0 0 0"/>
251
+ </visual> -->
252
+ </link>
253
+ <joint name="tool0_fixed_joint-tool_tip" type="fixed">
254
+ <origin rpy="0 0 0" xyz="0 0 0.175"/>
255
+ <parent link="tool0"/>
256
+ <child link="tool_tip"/>
257
+ </joint>
258
+
259
+ <link name="world"/>
260
+ <joint name="world_joint" type="fixed">
261
+ <parent link="world"/>
262
+ <child link="rotated_base_link"/>
263
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
264
+ </joint>
265
+
266
+ <link name="rotated_base_link">
267
+ <inertial>
268
+ <mass value="0.0"/>
269
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
270
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
271
+ </inertial>
272
+ </link>>
273
+
274
+ <joint name="rotated_base-base_fixed_joint" type="fixed">
275
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 3.141592653589793"/>
276
+ <parent link="rotated_base_link"/>
277
+ <child link="base_link"/>
278
+ </joint>
279
+ </robot>
ur5e/visual/base.dae ADDED
The diff for this file is too large to render. See raw diff
 
ur5e/visual/forearm.dae ADDED
The diff for this file is too large to render. See raw diff
 
ur5e/visual/shoulder.dae ADDED
The diff for this file is too large to render. See raw diff