RigidBodyPlanningWithControls.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39from math import sin, cos
40from functools import partial
41try:
42 from ompl import base as ob
43 from ompl import control as oc
44except ImportError:
45 # if the ompl module is not in the PYTHONPATH assume it is installed in a
46 # subdirectory of the parent directory called "py-bindings."
47 from os.path import abspath, dirname, join
48 import sys
49 sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
50 from ompl import base as ob
51 from ompl import control as oc
52
53
55class MyDecomposition(oc.GridDecomposition):
56 def __init__(self, length, bounds):
57 super(MyDecomposition, self).__init__(length, 2, bounds)
58 def project(self, s, coord):
59 coord[0] = s.getX()
60 coord[1] = s.getY()
61 def sampleFullState(self, sampler, coord, s):
62 sampler.sampleUniform(s)
63 s.setXY(coord[0], coord[1])
64
65
66def isStateValid(spaceInformation, state):
67 # perform collision checking or check if other constraints are
68 # satisfied
69 return spaceInformation.satisfiesBounds(state)
70
71def propagate(start, control, duration, state):
72 state.setX(start.getX() + control[0] * duration * cos(start.getYaw()))
73 state.setY(start.getY() + control[0] * duration * sin(start.getYaw()))
74 state.setYaw(start.getYaw() + control[1] * duration)
75
76def plan():
77 # construct the state space we are planning in
78 space = ob.SE2StateSpace()
79
80 # set the bounds for the R^2 part of SE(2)
81 bounds = ob.RealVectorBounds(2)
82 bounds.setLow(-1)
83 bounds.setHigh(1)
84 space.setBounds(bounds)
85
86 # create a control space
87 cspace = oc.RealVectorControlSpace(space, 2)
88
89 # set the bounds for the control space
90 cbounds = ob.RealVectorBounds(2)
91 cbounds.setLow(-.3)
92 cbounds.setHigh(.3)
93 cspace.setBounds(cbounds)
94
95 # define a simple setup class
96 ss = oc.SimpleSetup(cspace)
97 ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
98 partial(isStateValid, ss.getSpaceInformation())))
99 ss.setStatePropagator(oc.StatePropagatorFn(propagate))
100
101 # create a start state
102 start = ob.State(space)
103 start().setX(-0.5)
104 start().setY(0.0)
105 start().setYaw(0.0)
106
107 # create a goal state
108 goal = ob.State(space)
109 goal().setX(0.0)
110 goal().setY(0.5)
111 goal().setYaw(0.0)
112
113 # set the start and goal states
114 ss.setStartAndGoalStates(start, goal, 0.05)
115
116 # (optionally) set planner
117 si = ss.getSpaceInformation()
118 #planner = oc.RRT(si)
119 #planner = oc.EST(si)
120 #planner = oc.KPIECE1(si) # this is the default
121 # SyclopEST and SyclopRRT require a decomposition to guide the search
122 decomp = MyDecomposition(32, bounds)
123 planner = oc.SyclopEST(si, decomp)
124 #planner = oc.SyclopRRT(si, decomp)
125 ss.setPlanner(planner)
126 # (optionally) set propagation step size
127 si.setPropagationStepSize(.1)
128
129 # attempt to solve the problem
130 solved = ss.solve(20.0)
131
132 if solved:
133 # print the path to screen
134 print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
135
136if __name__ == "__main__":
137 plan()
The lower and upper bounds for an Rn space.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
Definition of an abstract state.
Definition: State.h:50
A GridDecomposition is a Decomposition implemented using a grid.
A control space representing Rn.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
SyclopEST is Syclop with EST as its low-level tree planner. .
Definition: SyclopEST.h:52
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.