SE3MultiRigidBodyPlanning.py
1 #!/usr/bin/env python
2 
3 
12 
13 # Author: Ryan Luna
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 geometric as og
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 geometric as og
28  from ompl import app as oa
29 
30 # plan in SE(3) for two robots
31 setup = oa.SE3MultiRigidBodyPlanning(2)
32 
33 # load the robots and the environment
34 setup.setRobotMesh('3D/cubicles_robot.dae')
35 setup.addRobotMesh('3D/cubicles_robot.dae')
36 setup.setEnvironmentMesh('3D/cubicles_env.dae')
37 
38 # define start state
39 start = ob.State(setup.getSpaceInformation())
40 # set start for robot 1
41 start1 = start()[0]
42 start1.setX(-4.96)
43 start1.setY(-40.62)
44 start1.setZ(70.57)
45 start1.rotation().setIdentity()
46 # set start for robot 2
47 start2 = start()[1]
48 start2.setX(200.49)
49 start2.setY(-40.62)
50 start2.setZ(70.57)
51 start2.rotation().setIdentity()
52 
53 # define goal state
54 goal = ob.State(setup.getSpaceInformation())
55 # set goal for robot 1
56 goal1 = goal()[0]
57 goal1.setX(200.49)
58 goal1.setY(-40.62)
59 goal1.setZ(70.57)
60 goal1.rotation().setIdentity()
61 # set goal for robot 2
62 goal2 = goal()[1]
63 goal2.setX(-4.96)
64 goal2.setY(-40.62)
65 goal2.setZ(70.57)
66 goal2.rotation().setIdentity()
67 
68 # set the start & goal states
69 setup.setStartAndGoalStates(start, goal)
70 
71 # setting collision checking resolution to 1% of the space extent
72 setup.getSpaceInformation().setStateValidityCheckingResolution(0.01)
73 
74 # use RRTConnect for planning
75 setup.setPlanner(og.RRTConnect(setup.getSpaceInformation()))
76 
77 # we call setup just so print() can show more information
78 setup.setup()
79 print(setup)
80 
81 # try to solve the problem
82 if setup.solve(60):
83  # simplify & print the solution
84  setup.simplifySolution()
85  print(setup.getSolutionPath().printAsMatrix())
Definition of an abstract state.
Definition: State.h:50
RRT-Connect (RRTConnect)
Definition: RRTConnect.h:62