Skip to content

Commit

Permalink
improve prm speed with dedicated C implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
wrangelvid committed Jul 24, 2023
1 parent 6737905 commit ab07721
Show file tree
Hide file tree
Showing 14 changed files with 1,314 additions and 653 deletions.
1 change: 0 additions & 1 deletion .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
/bazel-*
/build/
/cmake-build-*/
/data/
/.vscode

models/room_gen/building.sdf
Expand Down
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ from inside this repository.
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.

```
git clone -b gcs2 git@github.com:mpetersen94/drake.git
git clone -b gcs_paper git@github.com:wrangelvid/drake.git
mkdir drake-build
cd drake-build
cmake -DWITH_MOSEK=ON [-DWITH_GUROBI=ON -DWITH_ROBOTLOCOMOTION_SNOPT=ON] ../drake
Expand Down
Binary file added data/prm_comparison/IRIS.reg
View file
Binary file not shown.
Binary file added data/prm_comparison/roadmap.rmp
View file
Binary file not shown.
4 changes: 2 additions & 2 deletions examples/uav_planning.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@
"from reproduction.uav.building_generation import *\n",
"from reproduction.util import *\n",
"\n",
"g_lic = GurobiSolver.AcquireLicense()\n",
"m_lic = MosekSolver.AcquireLicense()"
"gurobi_license = GurobiSolver.AcquireLicense()\n",
"mosek_license = MosekSolver.AcquireLicense()"
]
},
{
Expand Down
2 changes: 1 addition & 1 deletion models/iiwa14_spheres_collision_welded_gripper.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ directives:
# Add schunk
- add_model:
name: wsg
file: package://gcs/models/schunk_wsg_50_welded_fingers.sdf
file: package://gcs/models/schunk_wsg_50_welded_fingers_sphere_collision.sdf

- add_frame:
name: iiwa::wsg_attach
Expand Down
89 changes: 89 additions & 0 deletions models/iiwa14_welded_gripper.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
# Model directive for iiwa with sphere collision and welded gripper
directives:

# Add iiwa
- add_model:
name: iiwa
file: package://drake/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf

- add_weld:
parent: world
child: iiwa::base

# Add schunk
- add_model:
name: wsg
file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_welded_fingers.sdf

- add_frame:
name: iiwa::wsg_attach
X_PF:
base_frame: iiwa::iiwa_link_7
translation: [0, 0, 0.114]
rotation: !Rpy { deg: [90.0, 0.0, 0.0 ]}

- add_weld:
parent: iiwa::wsg_attach
child: wsg::body

# Add shelves
- add_model:
name: shelves
file: package://gcs/models/shelves/shelves.sdf

- add_frame:
name: shelf_origin
X_PF:
base_frame: world
translation: [0.85, 0, 0.4]

- add_weld:
parent: shelf_origin
child: shelves::shelves_body

# Add Bins
- add_model:
name: binR
file: package://gcs/models/bin/bin.sdf

- add_frame:
name: bin_originR
X_PF:
base_frame: world
translation: [0, -0.6, 0]
rotation: !Rpy { deg: [0.0, 0.0, 90.0 ]}

- add_weld:
parent: bin_originR
child: binR::bin_base

- add_model:
name: binL
file: package://gcs/models/bin/bin.sdf

- add_frame:
name: bin_originL
X_PF:
base_frame: world
translation: [0, 0.6, 0]
rotation: !Rpy { deg: [0.0, 0.0, 90.0 ]}

- add_weld:
parent: bin_originL
child: binL::bin_base

# Add table
- add_model:
name: table
file: package://gcs/models/table/table_wide.sdf

- add_frame:
name: table_origin
X_PF:
base_frame: world
translation: [0.4, 0.0, 0.0]
rotation: !Rpy { deg: [0., 0., 00]}

- add_weld:
parent: table_origin
child: table::table_body
208 changes: 208 additions & 0 deletions models/schunk_wsg_50_welded_fingers_sphere_collision.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,208 @@
<?xml version="1.0"?>
<!-- This sdf file is based on schunk_wsg_50.sdf -->
<sdf version="1.7">
<model name="Schunk_Gripper">
<link name="body">
<pose>0 -0.049133 0 0 0 0</pose>
<inertial>
<mass>0.988882</mass>
<inertia>
<ixx>0.162992</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.162992</iyy>
<iyz>0</iyz>
<izz>0.164814</izz>
</inertia>
</inertial>
<kinematic>0</kinematic>
<visual name="visual">
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>package://drake_models/wsg_50_description/meshes/wsg_body.obj</uri>
</mesh>
</geometry>
<material>
<diffuse>0.7 0.7 0.7 1</diffuse>
</material>
</visual>
<collision name='sphere1'>
<pose>0.02 0 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
<collision name='sphere2'>
<pose>-0.02 0 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
<collision name='sphere3'>
<pose>-0.045 0.005 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
<collision name='sphere4'>
<pose>0.045 0.005 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
</link>
<frame name="body_frame">
<pose relative_to="body"/>
</frame>
<link name="left_finger">
<pose>-0.06 0.028 0 0 3.141592 0</pose>
<inertial>
<mass>0.05</mass>
<inertia>
<ixx>0.16</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.16</iyy>
<iyz>0</iyz>
<izz>0.16</izz>
</inertia>
</inertial>
<kinematic>0</kinematic>
<visual name="visual">
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>package://drake_models/wsg_50_description/meshes/finger_without_tip.obj</uri>
</mesh>
</geometry>
<material>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
<collision name="sphere1">
<pose>0.005 -0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.018</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere2">
<pose>0.005 -0.01 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.017</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere3">
<pose>0.002 0.005 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.014</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere4">
<pose>0.00 0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere5">
<pose>0.00 0.032 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.011</radius>
</sphere>
</geometry>
</collision>
</link>
<link name="right_finger">
<pose>0.06 0.028 0 0 0 0</pose>
<inertial>
<mass>0.05</mass>
<inertia>
<ixx>0.16</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.16</iyy>
<iyz>0</iyz>
<izz>0.16</izz>
</inertia>
</inertial>
<kinematic>0</kinematic>
<visual name="visual">
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>package://drake_models/wsg_50_description/meshes/finger_without_tip.obj</uri>
</mesh>
</geometry>
<material>
<diffuse>0.2 0.2 0.2 1</diffuse>
</material>
</visual>
<collision name="sphere1">
<pose>0.005 -0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.018</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere2">
<pose>0.005 -0.01 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.017</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere3">
<pose>0.002 0.005 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.014</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere4">
<pose>0.00 0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</collision>
<collision name="sphere5">
<pose>0.00 0.032 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.011</radius>
</sphere>
</geometry>
</collision>
</link>
<joint name="left_finger_sliding_joint" type="fixed">
<parent>body</parent>
<child>left_finger</child>
</joint>
<joint name="right_finger_sliding_joint" type="fixed">
<parent>body</parent>
<child>right_finger</child>
</joint>
<static>0</static>
</model>
</sdf>
4 changes: 2 additions & 2 deletions reproduction/bimanual/bimanual_iiwa_example.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,8 @@
"from reproduction.bimanual.helpers import *\n",
"from reproduction.util import *\n",
"\n",
"GurobiSolver.AcquireLicense()\n",
"MosekSolver.AcquireLicense()"
"gurobi_license = GurobiSolver.AcquireLicense()\n",
"mosek_license = MosekSolver.AcquireLicense()"
]
},
{
Expand Down
4 changes: 2 additions & 2 deletions reproduction/bimanual/helpers.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@
from gcs.bezier import BezierGCS
from gcs.linear import LinearGCS
from gcs.rounding import *
from reproduction.prm_comparison.helpers import lower_alpha
from reproduction.prm_comparison.helpers import set_transparency_of_models
from reproduction.util import *

def getIkSeeds():
Expand Down Expand Up @@ -467,7 +467,7 @@ def generate_segment_pics(traj, segment, meshcat):
arm_models = [iiwa1_start.model_instance, wsg1_start.model_instance,
iiwa2_start.model_instance, wsg2_start.model_instance,
iiwa1_goal, wsg1_goal, iiwa2_goal, wsg2_goal]
lower_alpha(plant, scene_graph.model_inspector(), arm_models, 0.4, scene_graph)
set_transparency_of_models(plant, arm_models, 0.4, scene_graph)

plant.Finalize()

Expand Down
Loading

0 comments on commit ab07721

Please sign in to comment.