SE3RigidBodyPlanning.py
1 #!/usr/bin/env python
2 
3 
12 
13 # Author: Mark Moll
14 
15 import sys
16 from os.path import abspath, dirname, join
17 
18 ompl_app_root = dirname(dirname(dirname(abspath(__file__))))
19 ompl_resources_dir = join(ompl_app_root, 'resources/3D')
20 
21 try:
22  from ompl import base as ob
23  from ompl import geometric as og
24  from ompl import control as oc
25  from ompl import app as oa
26 except:
27  sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings' ) )
28  from ompl import base as ob
29  from ompl import geometric as og
30  from ompl import control as oc
31  from ompl import app as oa
32 
33 # plan in SE(3)
34 setup = oa.SE3RigidBodyPlanning()
35 
36 # load the robot and the environment
37 setup.setRobotMesh(join(ompl_resources_dir, 'cubicles_robot.dae'))
38 setup.setEnvironmentMesh(join(ompl_resources_dir, 'cubicles_env.dae'))
39 
40 # define start state
41 start = ob.State(setup.getSpaceInformation())
42 start().setX(-4.96)
43 start().setY(-40.62)
44 start().setZ(70.57)
45 start().rotation().setIdentity()
46 
47 goal = ob.State(setup.getSpaceInformation())
48 goal().setX(200.49)
49 goal().setY(-40.62)
50 goal().setZ(70.57)
51 goal().rotation().setIdentity()
52 
53 # set the start & goal states
54 setup.setStartAndGoalStates(start, goal);
55 
56 # setting collision checking resolution to 1% of the space extent
57 setup.getSpaceInformation().setStateValidityCheckingResolution(0.01);
58 
59 # we call setup just so print() can show more information
60 setup.setup()
61 print(setup)
62 
63 # try to solve the problem
64 if setup.solve(10):
65  # simplify & print the solution
66  setup.simplifySolution()
67  path = setup.getSolutionPath()
68  path.interpolate(10)
69  print(path.printAsMatrix())
70  print(path.check())
Definition of an abstract state.
Definition: State.h:49