def __init__(self, ppm_file, obs: list, start=(1, 1), goal=(1000, 1000)):
self.ppm_ = ou.PPM()
self.ppm_.loadFile(ppm_file)
self.width = self.ppm_.getWidth()
self.height = self.ppm_.getHeight()
self.space = ob.RealVectorStateSpace()
self.space.addDimension(0.0, self.width)
self.space.addDimension(0.0, self.height)
self.bounds = ob.RealVectorBounds(2)
self.bounds.setLow(0)
self.bounds.setHigh(1024)
self.space.setBounds(self.bounds)
self.cspace = oc.RealVectorControlSpace(self.space, 2)
self.cbounds = ob.RealVectorBounds(2)
self.cbounds.setLow(-1)
self.cbounds.setHigh(1)
self.cspace.setBounds(self.cbounds)
self.setup = oc.SimpleSetup(self.cspace)
self.obs = obs
self.start = start
self.goal = goal
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
partial(isStateValid, ss.getSpaceInformation())))
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
#planner = oc.RRT(si)
#planner = oc.EST(si)
#planner = oc.KPIECE1(si) # this is the default
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, bounds)
planner = oc.SyclopEST(si, decomp)
#planner = oc.SyclopRRT(si, decomp)
ss.setPlanner(planner)
# (optionally) set propagation step size
si.setPropagationStepSize(.1)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def plan():
# construct the state space we are planning in
# The state space of the car is SE(2) (x and y position with one angle for orientation).
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
# ?? for this simple car model consists of the velocity and steering angle, both real valued.
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
global ss
ss = oc.SimpleSetup(cspace)
validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
#odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
odeSolver = oc.ODEErrorSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# attempt to solve the problem
solved = ss.solve(120.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# attempt to solve the problem
solved = ss.solve(120.0)
if solved:
# print the path to screen
print("Found solution:\n%s" %
ss.getSolutionPath().asGeometric().printAsMatrix())
def setSpace(self):
# construct the state space we are planning in
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
#self.bounds.setLow(low)
#self.bounds.setHigh(high)
self.bounds.setLow(0, ox)
self.bounds.setHigh(0, ox + size_x)
self.bounds.setLow(1, oy)
self.bounds.setHigh(1, oy + size_y)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
def setSpace(self):
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
self.bounds.setLow(low)
self.bounds.setHigh(high)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation()))
self.ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(self.kinematicCarODE)
odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
self.ss.setStatePropagator(propagator)
def __init__(self, robot_env: DifferentialDriveEnv):
self.robot_env = robot_env
bounds = self.robot_env.get_bounds()
self.state_bounds = bounds['state_bounds']
self.control_bounds = bounds['control_bounds']
# add the state space
space = ob.RealVectorStateSpace(len(self.state_bounds))
bounds = ob.RealVectorBounds(len(self.state_bounds))
# add state bounds
for k, v in enumerate(self.state_bounds):
bounds.setLow(k, float(v[0]))
bounds.setHigh(k, float(v[1]))
space.setBounds(bounds)
self.space = space
# add the control space
cspace = oc.RealVectorControlSpace(space, len(self.control_bounds))
# set the bounds for the control space
cbounds = ob.RealVectorBounds(len(self.control_bounds))
for k, v in enumerate(self.control_bounds):
cbounds.setLow(k, float(v[0]))
cbounds.setHigh(k, float(v[1]))
cspace.setBounds(cbounds)
self.cspace = cspace
# define a simple setup class
self.ss = oc.SimpleSetup(cspace)
self.ss.setStateValidityChecker(ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
# set the planner
si = self.ss.getSpaceInformation()
planner = oc.SST(si)
self.ss.setPlanner(planner)
si.setPropagationStepSize(.1)
self.start = None
self.goal = None
self.planning_time = 0.0
self.path = None
def __init__(self, ndof, iksolver, step_size=0.05):
self.ndof = ndof
self.iksolver = iksolver
self.state_space = MyStateSpace(ndof)
self.control_space = MyControlSpace(self.state_space, ndof)
self.simple_setup = oc.SimpleSetup(self.control_space)
si = self.simple_setup.getSpaceInformation()
si.setPropagationStepSize(step_size)
si.setMinMaxControlDuration(1, 1)
si.setDirectedControlSamplerAllocator(
oc.DirectedControlSamplerAllocator(
directedControlSamplerAllocator))
vc = MyStateValidityChecker(self.simple_setup.getSpaceInformation(),
self.iksolver, ndof)
self.simple_setup.setStateValidityChecker(vc)
ob = MyOptimizationObjective(self.simple_setup.getSpaceInformation())
self.simple_setup.setOptimizationObjective(ob)
propagator = MyStatePropagator(self.simple_setup.getSpaceInformation(),
ndof)
self.simple_setup.setStatePropagator(propagator)
# self.planner = oc.KPIECE1(self.simple_setup.getSpaceInformation())
# self.planner.setup()
# ========= RRT planner ============
self.planner = og.RRTstar(self.simple_setup.getSpaceInformation())
p_goal = 0.0
self.planner.setGoalBias(p_goal)
self.planner.setRange(step_size)
IPython.embed()
# =========== PRM planner =============
# self.planner = og.PRM(self.simple_setup.getSpaceInformation())
self.simple_setup.setPlanner(self.planner)
def plan():
# construct the state space we are planning in
ripl = app.SE2RigidBodyPlanning()
ripl.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ripl.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
space = ripl.getStateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-55)
bounds.setHigh(55)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 3.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
# partial(isStateValid, ss.getSpaceInformation())))
sp_info = ss.getSpaceInformation()
extractor = ripl.getGeometricStateExtractor()
enabled = ripl.isSelfCollisionEnabled()
checker = ripl.allocStateValidityChecker(sp_info, extractor, enabled)
ss.setStateValidityChecker(checker)
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
# planner = oc.PDST(si)
# planner = oc.RRT(si)
# planner = oc.EST(si)
planner = oc.KPIECE1(si) # this is the default
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, bounds)
# planner = oc.SyclopEST(si, decomp)
# planner = oc.SyclopRRT(si, decomp)
ss.setPlanner(planner)
# (optionally) set propagation step size
si.setPropagationStepSize(.1)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
with open('path.txt', 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
def execute(self, env, time, pathLength, show = False):
result = True
sSpace = MyStateSpace()
sbounds = ob.RealVectorBounds(4)
# dimension 0 (x) spans between [0, width)
# dimension 1 (y) spans between [0, height)
# since sampling is continuous and we round down, we allow values until
# just under the max limit
# the resolution is 1.0 since we check cells only
sbounds.low = ou.vectorDouble()
sbounds.low.extend([0.0, 0.0, -MAX_VELOCITY, -MAX_VELOCITY])
sbounds.high = ou.vectorDouble()
sbounds.high.extend([float(env.width) - 0.000000001,
float(env.height) - 0.000000001,
MAX_VELOCITY, MAX_VELOCITY])
sSpace.setBounds(sbounds)
cSpace = oc.RealVectorControlSpace(sSpace, 2)
cbounds = ob.RealVectorBounds(2)
cbounds.low[0] = -MAX_VELOCITY
cbounds.high[0] = MAX_VELOCITY
cbounds.low[1] = -MAX_VELOCITY
cbounds.high[1] = MAX_VELOCITY
cSpace.setBounds(cbounds)
ss = oc.SimpleSetup(cSpace)
isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid))
ss.setStateValidityChecker(isValidFn)
propagator = MyStatePropagator(ss.getSpaceInformation())
ss.setStatePropagator(propagator)
planner = self.newplanner(ss.getSpaceInformation())
ss.setPlanner(planner)
# the initial state
start = ob.State(sSpace)
start()[0] = env.start[0]
start()[1] = env.start[1]
start()[2] = 0.0
start()[3] = 0.0
goal = ob.State(sSpace)
goal()[0] = env.goal[0]
goal()[1] = env.goal[1]
goal()[2] = 0.0
goal()[3] = 0.0
ss.setStartAndGoalStates(start, goal, 0.05)
startTime = clock()
if ss.solve(SOLUTION_TIME):
elapsed = clock() - startTime
time = time + elapsed
if show:
print('Found solution in %f seconds!' % elapsed)
path = ss.getSolutionPath()
path.interpolate()
if not path.check():
return (False, time, pathLength)
pathLength = pathLength + path.length()
if show:
print(env, '\n')
temp = copy.deepcopy(env)
for i in range(len(path.states)):
x = int(path.states[i][0])
y = int(path.states[i][1])
if temp.grid[x][y] in [0,2]:
temp.grid[x][y] = 2
else:
temp.grid[x][y] = 3
print(temp, '\n')
else:
result = False
return (result, time, pathLength)
def plan(run_time, planner_type, objective_type, wind_direction, dimensions,
obstacles):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, dimensions[0])
bounds.setLow(1, dimensions[1])
bounds.setHigh(0, dimensions[2])
bounds.setHigh(1, dimensions[3])
# Set the bounds of space to be in [0,1].
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 1)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(1)
cbounds.setLow(-math.pi / 3)
cbounds.setHigh(math.pi / 3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
propagator = get_state_propagator(wind_direction)
ss.setStatePropagator(oc.StatePropagatorFn(propagator))
# Construct a space information instance for this state space
si = ss.getSpaceInformation()
si.setPropagationStepSize(1)
# Set the object used to check which states in the space are valid
validity_checker = ValidityChecker(si, obstacles)
ss.setStateValidityChecker(validity_checker)
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = dimensions[0]
start[1] = dimensions[1]
start[2] = math.pi / 4
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = dimensions[2]
goal[1] = dimensions[3]
goal[2] = math.pi / 4
# Set the start and goal states
ss.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
objective = allocate_objective(si, objective_type)
ss.setOptimizationObjective(objective)
# Create a decomposition of the state space
decomp = MyDecomposition(256, bounds)
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizing_planner = allocate_planner(si, planner_type, decomp)
ss.setPlanner(optimizing_planner)
# attempt to solve the planning problem in the given runtime
solved = ss.solve(run_time)
if solved:
# Output the length of the path found
print('{0} found solution of path length {1:.4f} with an optimization '
'objective value of {2:.4f}'.format(
ss.getPlanner().getName(),
ss.getSolutionPath().length(), 0.1))
print(ss.getSolutionPath().printAsMatrix())
print(ss.haveExactSolutionPath())
plot_path(ss.getSolutionPath(), dimensions, obstacles)
else:
print("No solution found.")
def __init__(
self,
fwd_model: BaseDynamicsFunction,
filter_model: BaseFilterFunction,
classifier_model: BaseConstraintChecker,
planner_params: Dict,
action_params: Dict,
scenario: ExperimentScenario,
verbose: int,
):
super().__init__(scenario=scenario,
fwd_model=fwd_model,
filter_model=filter_model)
self.verbose = verbose
self.fwd_model = fwd_model
self.classifier_model = classifier_model
self.params = planner_params
self.action_params = action_params
# TODO: consider making full env params h/w come from elsewhere. res should match the model though.
self.si = ob.SpaceInformation(ob.StateSpace())
self.classifier_rng = np.random.RandomState(0)
self.state_sampler_rng = np.random.RandomState(0)
self.goal_sampler_rng = np.random.RandomState(0)
self.control_sampler_rng = np.random.RandomState(0)
self.scenario = scenario
self.scenario_ompl = get_ompl_scenario(self.scenario)
self.state_space = self.scenario_ompl.make_ompl_state_space(
planner_params=self.params,
state_sampler_rng=self.state_sampler_rng,
plot=self.verbose >= 2)
# self.state_space.sanityChecks()
self.control_space = self.scenario_ompl.make_ompl_control_space(
self.state_space,
self.control_sampler_rng,
action_params=self.action_params)
self.ss = oc.SimpleSetup(self.control_space)
self.si = self.ss.getSpaceInformation()
self.ss.setStatePropagator(oc.AdvancedStatePropagatorFn(
self.propagate))
self.ss.setMotionsValidityChecker(
oc.MotionsValidityCheckerFn(self.motions_valid))
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_valid))
self.cleanup_before_plan(0)
# just for debugging
self.cc = CollisionCheckerClassifier(
[pathlib.Path("cl_trials/cc_baseline/cc")], self.scenario, 0.0)
self.cc_but_accept_count = 0
self.rrt = oc.RRT(self.si)
self.rrt.setIntermediateStates(
True
) # this is necessary, because we use this to generate datasets
self.ss.setPlanner(self.rrt)
self.si.setMinMaxControlDuration(1, 50)
def __init__(self,
map_,
pose,
target,
max_time=120,
allow_moving_backward=True,
omega_max=0.5,
vmax=0.08,
objective='duration',
planner=oc.SST,
threshold=0.9,
tolerance=0.3,
control_duration=1,
k=1.0,
min_y=None,
max_y=None):
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(map_.size)
if min_y is not None:
bounds.setLow(min_y)
if max_y is not None:
bounds.setHigh(max_y)
space.setBounds(bounds)
cspace = oc.RealVectorControlSpace(space, 2)
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-omega_max)
cbounds.setHigh(omega_max)
cspace.setBounds(cbounds)
ss = oc.SimpleSetup(cspace)
si = ss.getSpaceInformation()
ss.setStateValidityChecker(
ob.StateValidityCheckerFn(partial(valid, si)))
self.propagation_data = {'t': 0, 'nt': 0}
ss.setStatePropagator(
oc.StatePropagatorFn(
partial(propagate, self.propagation_data, map_, threshold,
allow_moving_backward)))
start = ob.State(space)
state_from_tuple(start(), pose)
goal = ob.State(space)
state_from_tuple(goal(), target)
ss.setStartAndGoalStates(start, goal, tolerance)
p = planner(si)
ss.setPlanner(p)
si.setPropagationStepSize(control_duration)
si.setMinMaxControlDuration(control_duration, control_duration)
if objective == 'duration':
do = DurationObjective(si)
elif objective == 'balanced':
do = getBalancedObjective1(si, k=k)
else:
do = ob.PathLengthOptimizationObjective(si)
ss.setOptimizationObjective(do)
ss.setup()
self.ss = ss
self.map = map_
self.s = pose
self.t = target
self.objective = objective
self.planner_name = 'SST'
self.planner_options = {}
self.threshold = threshold
self.tolerance = tolerance
self.comp_duration = 0
self.max_time = max_time
self.allow_moving_backward = allow_moving_backward
def plan(self,goalPoints,current_region,next_region,samplerIndex):
"""
goal points: array that contains the coordinates of all the possible goal states
current_reg: name of the current region (p1 etc)
next_reg : name of the next region (p1 etc)
"""
# construct the state space we are planning in
if self.Space_Dimension == 2:
space = ob.SE2StateSpace()
else:
space = ob.SE3StateSpace()
# set the bounds
bounds = ob.RealVectorBounds(self.Space_Dimension)
BoundaryMaxMin = self.all.boundingBox() #0-3:xmin, xmax, ymin and ymax
bounds.setLow(0,BoundaryMaxMin[0]) # 0 stands for x axis
bounds.setHigh(0,BoundaryMaxMin[1])
bounds.setLow(1,BoundaryMaxMin[2]) # 1 stands for y axis
bounds.setHigh(1,BoundaryMaxMin[3])
if self.Space_Dimension == 3:
bounds.setLow(2,0) # 2 stands for z axis
bounds.setHigh(2,self.maxHeight)
space.setBounds(bounds)
if self.system_print == True:
print "The bounding box of the boundary is: " + str(self.all.boundingBox() )
print "The volume of the bounding box is: " + str(bounds.getVolume())
if self.Geometric_Control == 'C':
# create a control space
cspace = oc.RealVectorControlSpace(space, self.Space_Dimension)
# set the bounds for the control space
# cbounds[0] for velocity
# cbounds[1] for angular velocity
cbounds = ob.RealVectorBounds(self.Space_Dimension)
cbounds.setLow(0,0)
cbounds.setHigh(0,max((BoundaryMaxMin[1]-BoundaryMaxMin[0]),(BoundaryMaxMin[3]-BoundaryMaxMin[2]))/100)
cbounds.setLow(1,-pi/5)
cbounds.setHigh(1,pi/5)
cspace.setBounds(cbounds)
if self.system_print == True:
print cspace.settings()
if self.Geometric_Control == 'G':
# define a simple setup class
ss = og.SimpleSetup(space)
else:
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# set state validity checking for this space
ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
# create a start state
start = ob.State(space)
pose = self.pose_handler.getPose() #x,y,w,(z if using ROS quadrotor)
start().setX(pose[0])
start().setY(pose[1])
if self.Space_Dimension == 2:
while pose[2] > pi or pose[2] < -pi:
if pose[2]> pi:
pose[2] = pose[2] - pi
else:
pose[2] = pose[2] + pi
start().setYaw(pose[2]) #
else:
start().setZ(pose[3])
start().rotation().setIdentity()
if self.system_print is True and self.Space_Dimension == 3:
print "start:" + str(start().getX())+","+str(start().getY()) +"," + str(start().getZ())
print goalPoints
# create goal states
goalStates = ob.GoalStates(ss.getSpaceInformation())
for i in range(shape(goalPoints)[1]):
goal = ob.State(space)
goal().setX(goalPoints[0,i])
goal().setY(goalPoints[1,i])
if self.Space_Dimension == 3:
if self.system_print is True:
print current_region,next_region
print self.map['height'][current_region],self.map['height'][next_region]
z_goalPoint = min(self.map['height'][current_region]/2,self.map['height'][next_region]/2)
goal().setZ(z_goalPoint)
goal().rotation().setIdentity()
else:
goal().setYaw(0.0)
if self.plotting == True:
if self.Space_Dimension == 3:
self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],[z_goalPoint],'ro')
else:
self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],'ro')
self.setPlotLimitXYZ()
self.ax.get_figure().canvas.draw()
goalStates.addState(goal)
if self.system_print is True:
print goalStates
# set the start and goal states;
ss.setGoal(goalStates)
ss.setStartState(start)
# set sampler (optional; the default is uniform sampling)
si = ss.getSpaceInformation()
# set planner
planner_prep = self.planner_dictionary[self.Geometric_Control][self.planner]
planner = planner_prep(si)
ss.setPlanner(planner)
if self.Geometric_Control == 'G':
if not self.planner == 'PRM':
planner.setRange(self.radius*2)
if self.system_print is True:
print "planner.getRange():" + str(planner.getRange())
#if not self.planner == 'RRTConnect':
# planner.setGoalBias(0.5)
else:
# (optionally) set propagation step size
si.setPropagationStepSize(1) #actually is the duration in propagate
si.setMinMaxControlDuration(3,3) # is the no of steps taken with the same velocity and omega
if self.system_print is True:
print "radius: " +str(self.radius)
print "si.getPropagationStepSize():" + str(si.getPropagationStepSize())
planner.setGoalBias(0.5)
ss.setup()
# attempt to solve the problem within ten seconds of planning time
solved = ss.solve(1000.0) #10
if (solved):
print("Found solution:")
# print the path to screen
print >>sys.__stdout__,(ss.getSolutionPath())
else:
print("No solution found")
if self.plotting == True :
self.ax.set_title('Map with Geo/Control: '+str(self.Geometric_Control) + ",Planner:" +str(self.planner), fontsize=12)
self.ax.set_xlabel('x')
self.ax.set_ylabel('y')
for i in range(ss.getSolutionPath().getStateCount()-1):
if self.Space_Dimension == 3:
self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),((ss.getSolutionPath().getState(i)).getZ(),(ss.getSolutionPath().getState(i+1)).getZ()),'b')
ro=Polygon.Shapes.Circle (self.radius,((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i)).getY()))
self.plotPoly(ro,'r')
else:
self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),0,'b')
self.setPlotLimitXYZ()
#self.ax.get_figure().canvas.draw()
return ss
