Skip to content

Commit bdfe94f

Browse files
wrangelvidwrangelvid
authored
Merge pull request #49 from mpetersen94/prm_comparison_update
PRM comparison update
2 parents b32e5fd + ab07721 commit bdfe94f

15 files changed

+1321
-660
lines changed

‎.gitignore‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,6 @@
33
/bazel-*
44
/build/
55
/cmake-build-*/
6-
/data/
76
/.vscode
87

98
models/room_gen/building.sdf

‎README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ from inside this repository.
3232
If you want to compare GCS to sampling based planners (such as PRM), you'll need to install a custom fork of drake that includes bindings for sampling based planners. To do this run the following, including any of the proprietary solvers you have access to.
3333

3434
```
35-
git clone -b gcs2 [email protected]:mpetersen94/drake.git
35+
git clone -b gcs_paper [email protected]:wrangelvid/drake.git
3636
mkdir drake-build
3737
cd drake-build
3838
cmake -DWITH_MOSEK=ON [-DWITH_GUROBI=ON -DWITH_ROBOTLOCOMOTION_SNOPT=ON] ../drake

‎data/prm_comparison/IRIS.reg‎

43.8 KB
Binary file not shown.

‎data/prm_comparison/roadmap.rmp‎

4.08 MB
Binary file not shown.

‎examples/uav_planning.ipynb‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,8 @@
3434
"from reproduction.uav.building_generation import *\n",
3535
"from reproduction.util import *\n",
3636
"\n",
37-
"g_lic = GurobiSolver.AcquireLicense()\n",
38-
"m_lic = MosekSolver.AcquireLicense()"
37+
"gurobi_license = GurobiSolver.AcquireLicense()\n",
38+
"mosek_license = MosekSolver.AcquireLicense()"
3939
]
4040
},
4141
{

‎models/iiwa14_spheres_collision_welded_gripper.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ directives:
1313
# Add schunk
1414
- add_model:
1515
name: wsg
16-
file: package://gcs/models/schunk_wsg_50_welded_fingers.sdf
16+
file: package://gcs/models/schunk_wsg_50_welded_fingers_sphere_collision.sdf
1717

1818
- add_frame:
1919
name: iiwa::wsg_attach

‎models/iiwa14_welded_gripper.yaml‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
# Model directive for iiwa with sphere collision and welded gripper
2+
directives:
3+
4+
# Add iiwa
5+
- add_model:
6+
name: iiwa
7+
file: package://drake/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf
8+
9+
- add_weld:
10+
parent: world
11+
child: iiwa::base
12+
13+
# Add schunk
14+
- add_model:
15+
name: wsg
16+
file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_welded_fingers.sdf
17+
18+
- add_frame:
19+
name: iiwa::wsg_attach
20+
X_PF:
21+
base_frame: iiwa::iiwa_link_7
22+
translation: [0, 0, 0.114]
23+
rotation: !Rpy { deg: [90.0, 0.0, 0.0 ]}
24+
25+
- add_weld:
26+
parent: iiwa::wsg_attach
27+
child: wsg::body
28+
29+
# Add shelves
30+
- add_model:
31+
name: shelves
32+
file: package://gcs/models/shelves/shelves.sdf
33+
34+
- add_frame:
35+
name: shelf_origin
36+
X_PF:
37+
base_frame: world
38+
translation: [0.85, 0, 0.4]
39+
40+
- add_weld:
41+
parent: shelf_origin
42+
child: shelves::shelves_body
43+
44+
# Add Bins
45+
- add_model:
46+
name: binR
47+
file: package://gcs/models/bin/bin.sdf
48+
49+
- add_frame:
50+
name: bin_originR
51+
X_PF:
52+
base_frame: world
53+
translation: [0, -0.6, 0]
54+
rotation: !Rpy { deg: [0.0, 0.0, 90.0 ]}
55+
56+
- add_weld:
57+
parent: bin_originR
58+
child: binR::bin_base
59+
60+
- add_model:
61+
name: binL
62+
file: package://gcs/models/bin/bin.sdf
63+
64+
- add_frame:
65+
name: bin_originL
66+
X_PF:
67+
base_frame: world
68+
translation: [0, 0.6, 0]
69+
rotation: !Rpy { deg: [0.0, 0.0, 90.0 ]}
70+
71+
- add_weld:
72+
parent: bin_originL
73+
child: binL::bin_base
74+
75+
# Add table
76+
- add_model:
77+
name: table
78+
file: package://gcs/models/table/table_wide.sdf
79+
80+
- add_frame:
81+
name: table_origin
82+
X_PF:
83+
base_frame: world
84+
translation: [0.4, 0.0, 0.0]
85+
rotation: !Rpy { deg: [0., 0., 00]}
86+
87+
- add_weld:
88+
parent: table_origin
89+
child: table::table_body

‎models/schunk_wsg_50_welded_fingers.sdf‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
<geometry>
2121
<mesh>
2222
<scale>1 1 1</scale>
23-
<uri>package://drake/manipulation/models/wsg_50_description/meshes/wsg_body.obj</uri>
23+
<uri>package://drake_models/wsg_50_description/meshes/wsg_body.obj</uri>
2424
</mesh>
2525
</geometry>
2626
<material>
@@ -102,7 +102,7 @@
102102
<geometry>
103103
<mesh>
104104
<scale>1 1 1</scale>
105-
<uri>package://drake/manipulation/models/wsg_50_description/meshes/finger_without_tip.obj</uri>
105+
<uri>package://drake_models/wsg_50_description/meshes/finger_without_tip.obj</uri>
106106
</mesh>
107107
</geometry>
108108
<material>
@@ -181,7 +181,7 @@
181181
<geometry>
182182
<mesh>
183183
<scale>1 1 1</scale>
184-
<uri>package://drake/manipulation/models/wsg_50_description/meshes/finger_without_tip.obj</uri>
184+
<uri>package://drake_models/wsg_50_description/meshes/finger_without_tip.obj</uri>
185185
</mesh>
186186
</geometry>
187187
<material>

‎models/schunk_wsg_50_welded_fingers_sphere_collision.sdf‎

Lines changed: 208 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
<?xml version="1.0"?>
2+
<!-- This sdf file is based on schunk_wsg_50.sdf -->
3+
<sdf version="1.7">
4+
<model name="Schunk_Gripper">
5+
<link name="body">
6+
<pose>0 -0.049133 0 0 0 0</pose>
7+
<inertial>
8+
<mass>0.988882</mass>
9+
<inertia>
10+
<ixx>0.162992</ixx>
11+
<ixy>0</ixy>
12+
<ixz>0</ixz>
13+
<iyy>0.162992</iyy>
14+
<iyz>0</iyz>
15+
<izz>0.164814</izz>
16+
</inertia>
17+
</inertial>
18+
<kinematic>0</kinematic>
19+
<visual name="visual">
20+
<geometry>
21+
<mesh>
22+
<scale>1 1 1</scale>
23+
<uri>package://drake_models/wsg_50_description/meshes/wsg_body.obj</uri>
24+
</mesh>
25+
</geometry>
26+
<material>
27+
<diffuse>0.7 0.7 0.7 1</diffuse>
28+
</material>
29+
</visual>
30+
<collision name='sphere1'>
31+
<pose>0.02 0 0.0 0 0 0</pose>
32+
<geometry>
33+
<sphere>
34+
<radius>0.05</radius>
35+
</sphere>
36+
</geometry>
37+
</collision>
38+
<collision name='sphere2'>
39+
<pose>-0.02 0 0.0 0 0 0</pose>
40+
<geometry>
41+
<sphere>
42+
<radius>0.05</radius>
43+
</sphere>
44+
</geometry>
45+
</collision>
46+
<collision name='sphere3'>
47+
<pose>-0.045 0.005 0.0 0 0 0</pose>
48+
<geometry>
49+
<sphere>
50+
<radius>0.05</radius>
51+
</sphere>
52+
</geometry>
53+
</collision>
54+
<collision name='sphere4'>
55+
<pose>0.045 0.005 0.0 0 0 0</pose>
56+
<geometry>
57+
<sphere>
58+
<radius>0.05</radius>
59+
</sphere>
60+
</geometry>
61+
</collision>
62+
</link>
63+
<frame name="body_frame">
64+
<pose relative_to="body"/>
65+
</frame>
66+
<link name="left_finger">
67+
<pose>-0.06 0.028 0 0 3.141592 0</pose>
68+
<inertial>
69+
<mass>0.05</mass>
70+
<inertia>
71+
<ixx>0.16</ixx>
72+
<ixy>0</ixy>
73+
<ixz>0</ixz>
74+
<iyy>0.16</iyy>
75+
<iyz>0</iyz>
76+
<izz>0.16</izz>
77+
</inertia>
78+
</inertial>
79+
<kinematic>0</kinematic>
80+
<visual name="visual">
81+
<geometry>
82+
<mesh>
83+
<scale>1 1 1</scale>
84+
<uri>package://drake_models/wsg_50_description/meshes/finger_without_tip.obj</uri>
85+
</mesh>
86+
</geometry>
87+
<material>
88+
<diffuse>0.2 0.2 0.2 1</diffuse>
89+
</material>
90+
</visual>
91+
<collision name="sphere1">
92+
<pose>0.005 -0.02 0 0 0 0</pose>
93+
<geometry>
94+
<sphere>
95+
<radius>0.018</radius>
96+
</sphere>
97+
</geometry>
98+
</collision>
99+
<collision name="sphere2">
100+
<pose>0.005 -0.01 0 0 0 0</pose>
101+
<geometry>
102+
<sphere>
103+
<radius>0.017</radius>
104+
</sphere>
105+
</geometry>
106+
</collision>
107+
<collision name="sphere3">
108+
<pose>0.002 0.005 0 0 0 0</pose>
109+
<geometry>
110+
<sphere>
111+
<radius>0.014</radius>
112+
</sphere>
113+
</geometry>
114+
</collision>
115+
<collision name="sphere4">
116+
<pose>0.00 0.02 0 0 0 0</pose>
117+
<geometry>
118+
<sphere>
119+
<radius>0.01</radius>
120+
</sphere>
121+
</geometry>
122+
</collision>
123+
<collision name="sphere5">
124+
<pose>0.00 0.032 0 0 0 0</pose>
125+
<geometry>
126+
<sphere>
127+
<radius>0.011</radius>
128+
</sphere>
129+
</geometry>
130+
</collision>
131+
</link>
132+
<link name="right_finger">
133+
<pose>0.06 0.028 0 0 0 0</pose>
134+
<inertial>
135+
<mass>0.05</mass>
136+
<inertia>
137+
<ixx>0.16</ixx>
138+
<ixy>0</ixy>
139+
<ixz>0</ixz>
140+
<iyy>0.16</iyy>
141+
<iyz>0</iyz>
142+
<izz>0.16</izz>
143+
</inertia>
144+
</inertial>
145+
<kinematic>0</kinematic>
146+
<visual name="visual">
147+
<geometry>
148+
<mesh>
149+
<scale>1 1 1</scale>
150+
<uri>package://drake_models/wsg_50_description/meshes/finger_without_tip.obj</uri>
151+
</mesh>
152+
</geometry>
153+
<material>
154+
<diffuse>0.2 0.2 0.2 1</diffuse>
155+
</material>
156+
</visual>
157+
<collision name="sphere1">
158+
<pose>0.005 -0.02 0 0 0 0</pose>
159+
<geometry>
160+
<sphere>
161+
<radius>0.018</radius>
162+
</sphere>
163+
</geometry>
164+
</collision>
165+
<collision name="sphere2">
166+
<pose>0.005 -0.01 0 0 0 0</pose>
167+
<geometry>
168+
<sphere>
169+
<radius>0.017</radius>
170+
</sphere>
171+
</geometry>
172+
</collision>
173+
<collision name="sphere3">
174+
<pose>0.002 0.005 0 0 0 0</pose>
175+
<geometry>
176+
<sphere>
177+
<radius>0.014</radius>
178+
</sphere>
179+
</geometry>
180+
</collision>
181+
<collision name="sphere4">
182+
<pose>0.00 0.02 0 0 0 0</pose>
183+
<geometry>
184+
<sphere>
185+
<radius>0.01</radius>
186+
</sphere>
187+
</geometry>
188+
</collision>
189+
<collision name="sphere5">
190+
<pose>0.00 0.032 0 0 0 0</pose>
191+
<geometry>
192+
<sphere>
193+
<radius>0.011</radius>
194+
</sphere>
195+
</geometry>
196+
</collision>
197+
</link>
198+
<joint name="left_finger_sliding_joint" type="fixed">
199+
<parent>body</parent>
200+
<child>left_finger</child>
201+
</joint>
202+
<joint name="right_finger_sliding_joint" type="fixed">
203+
<parent>body</parent>
204+
<child>right_finger</child>
205+
</joint>
206+
<static>0</static>
207+
</model>
208+
</sdf>

‎reproduction/bimanual/bimanual_iiwa_example.ipynb‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,8 @@
3333
"from reproduction.bimanual.helpers import *\n",
3434
"from reproduction.util import *\n",
3535
"\n",
36-
"GurobiSolver.AcquireLicense()\n",
37-
"MosekSolver.AcquireLicense()"
36+
"gurobi_license = GurobiSolver.AcquireLicense()\n",
37+
"mosek_license = MosekSolver.AcquireLicense()"
3838
]
3939
},
4040
{
@@ -91,7 +91,7 @@
9191
"context = diagram.CreateDefaultContext()\n",
9292
"plant_context = plant.GetMyMutableContextFromRoot(context)\n",
9393
"sg_context = scene_graph.GetMyMutableContextFromRoot(context)\n",
94-
"diagram.Publish(context)"
94+
"diagram.ForcedPublish(context)"
9595
]
9696
},
9797
{

0 commit comments

Comments
 (0)