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 
20 try:
21  from ompl import base as ob
22  from ompl import app as oa
23 except ImportError:
24  sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings'))
25  from ompl import base as ob
26  from ompl import app as oa
27 
28 # plan in SE(3)
29 setup = oa.SE3RigidBodyPlanning()
30 
31 # load the robot and the environment
32 setup.setRobotMesh('3D/cubicles_robot.dae')
33 setup.setEnvironmentMesh('3D/cubicles_env.dae')
34 
35 # define start state
36 start = ob.State(setup.getSpaceInformation())
37 start().setX(-4.96)
38 start().setY(-40.62)
39 start().setZ(70.57)
40 start().rotation().setIdentity()
41 
42 goal = ob.State(setup.getSpaceInformation())
43 goal().setX(200.49)
44 goal().setY(-40.62)
45 goal().setZ(70.57)
46 goal().rotation().setIdentity()
47 
48 # set the start & goal states
49 setup.setStartAndGoalStates(start, goal)
50 
51 # setting collision checking resolution to 1% of the space extent
52 setup.getSpaceInformation().setStateValidityCheckingResolution(0.01)
53 
54 # we call setup just so print() can show more information
55 setup.setup()
56 print(setup)
57 
58 # try to solve the problem
59 if setup.solve(10):
60  # simplify & print the solution
61  setup.simplifySolution()
62  path = setup.getSolutionPath()
63  path.interpolate(10)
64  print(path.printAsMatrix())
65  print(path.check())
Definition of an abstract state.
Definition: State.h:50