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