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