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