def test_optimal(calc_time=0.01):
space = ob.RealVectorStateSpace(2)
space.setBounds(0, WIDTH)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ValidityChecker(si))
si.setup()
start = ob.State(space)
goal = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal[0] = WIDTH - 1
goal[1] = WIDTH - 1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
objective = ob.MultiOptimizationObjective(si)
objective.addObjective(MapCostObjective(si), 0.9)
objective.addObjective(ob.PathLengthOptimizationObjective(si), 0.1)
pdef.setOptimizationObjective(objective)
planner = og.RRTstar(si)
planner.setProblemDefinition(pdef)
planner.setup()
status = planner.solve(calc_time)
if pdef.hasExactSolution():
states = pdef.getSolutionPath().getStates()
cost = pdef.getSolutionPath().cost(objective).value()
states_np = np.array([(state[0], state[1]) for state in states])
print(states)
print(cost)
print(states_np)
def __init__(self, lower_bound, upper_bound):
"""Initialize the object."""
# Create an R2 state space
self.space = ob.RealVectorStateSpace(2)
# Set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(lower_bound)
bounds.setHigh(upper_bound)
self.space.setBounds(bounds)
# Create a new space information
self.si = ob.SpaceInformation(self.space)
# Set the state validity checker
self.si.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_state_valid))
self.si.setup()
# Set the problem definition
self.pdef = ob.ProblemDefinition(self.si)
# Setup an optimizing planner with RRT*
self.optimizingPlanner = og.RRTstar(self.si)
# Empty obstacles list
self.obstacles = []
def __init__(self, shape, calc_time, resolution, distance):
ou.setLogLevel(ou.LOG_WARN)
self._calc_time = calc_time
self._space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0.0)
bounds.setHigh(0, shape[0] - 1)
bounds.setHigh(1, shape[1] - 1)
self._space.setBounds(bounds)
self._si = ob.SpaceInformation(self._space)
self._validity_checker = OmplPlannerImpl.ValidityChecker(self._si)
self._si.setStateValidityChecker(self._validity_checker)
self._si.setStateValidityCheckingResolution(resolution)
self._si.setup()
self._start = ob.State(self._space)
self._goal = ob.State(self._space)
self._pdef = ob.ProblemDefinition(self._si)
objective = ob.MultiOptimizationObjective(self._si)
objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), 0.5)
# ## setting length objective
length_objective = ob.PathLengthOptimizationObjective(self._si)
objective.addObjective(length_objective, 0.1)
# objective.setCostThreshold(1000.0)
self._pdef.setOptimizationObjective(objective)
self._planner = og.RRTstar(self._si)
self._planner.setRange(distance)
self._planner.setup()
def setup_ompl(self):
print "set space"
self.space = ob.RealVectorStateSpace(dim=self.space_dimension)
bounds = ob.RealVectorBounds(self.space_dimension)
print "set bounds"
for i in xrange(self.space_dimension):
bounds.setLow(i, self.joint_constraints[0])
bounds.setHigh(i, self.joint_constraints[1])
print "set bounds 2"
self.space.setBounds(bounds)
print "set si"
self.si = ob.SpaceInformation(self.space)
print "set motion validator"
self.motion_validator = MotionValidator(self.si)
self.motion_validator.set_link_dimensions(self.link_dimensions)
self.motion_validator.set_workspace_dimension(self.workspace_dimension)
self.motion_validator.set_max_distance(self.max_velocity, self.delta_t)
print "set state validiy checker"
self.si.setStateValidityChecker(
ob.StateValidityCheckerFn(self.motion_validator.isValid))
print "set motion validator"
self.si.setMotionValidator(self.motion_validator)
print "si.setup"
self.si.setup()
self.problem_definition = ob.ProblemDefinition(self.si)
def planBITstar(q_st, q_g, res):
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
# set lower and upper bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(0)
bounds.setHigh(res)
space.setBounds(bounds)
# construct an instance of space information from this state space
si = ob.SpaceInformation(space)
# set state validity checking for this space
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create start state
start = ob.State(space)
start[0] = q_st[0]
start[1] = q_st[1]
# create goal state
goal = ob.State(space)
goal[0] = q_g[0]
goal[1] = q_g[1]
# create a problem instance
pdef = ob.ProblemDefinition(si)
# set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.BITstar(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# attempt to solve the problem within one second of planning time
solved = planner.solve(1.0)
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n")
path_arr = []
print(path.getStateCount())
for i in range(path.getStateCount()):
path_arr.append([])
path_arr[i].append(path.getState(i)[0])
path_arr[i].append(path.getState(i)[1])
return path_arr
else:
print("No solution found")
return []
def execute(self, env, time, pathLength, show=False):
result = True
# instantiate space information
si = mySpaceInformation(env)
# instantiate problem definition
pdef = ob.ProblemDefinition(si)
# instantiate motion planner
planner = self.newplanner(si)
planner.setProblemDefinition(pdef)
planner.setup()
# the initial state
state = ob.State(si)
state()[0] = env.start[0]
state()[1] = env.start[1]
pdef.addStartState(state)
goal = ob.GoalState(si)
gstate = ob.State(si)
gstate()[0] = env.goal[0]
gstate()[1] = env.goal[1]
goal.setState(gstate)
goal.threshold = 1e-3
pdef.setGoal(goal)
startTime = clock()
if planner.solve(SOLUTION_TIME):
elapsed = clock() - startTime
time = time + elapsed
if show:
print('Found solution in %f seconds!' % elapsed)
path = pdef.getSolutionPath()
sm = og.PathSimplifier(si)
startTime = clock()
sm.reduceVertices(path)
elapsed = clock() - startTime
time = time + elapsed
if show:
print('Simplified solution in %f seconds!' % elapsed)
path.interpolate(100)
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 _get_path(
self,
is_state_valid: Callable[[np.ndarray], bool],
start_js: np.ndarray,
robot_targ: RobotTarget,
use_sim: MpSim,
mp_space: MpSpace,
timeout: int,
):
"""
Does the low-level path planning with OMPL.
"""
if not self._should_render:
ou.setLogLevel(ou.LOG_ERROR)
dim = mp_space.get_state_dim()
space = ob.RealVectorStateSpace(dim)
bounds = ob.RealVectorBounds(dim)
lims = mp_space.get_state_lims()
for i, lim in enumerate(lims):
bounds.setLow(i, lim[0])
bounds.setHigh(i, lim[1])
space.setBounds(bounds)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid))
si.setup()
pdef = ob.ProblemDefinition(si)
mp_space.set_problem(pdef, space, si, start_js, robot_targ)
planner = mp_space.get_planner(si)
planner.setProblemDefinition(pdef)
planner.setup()
if mp_space.get_range() is not None:
planner.setRange(mp_space.get_range())
solved = planner.solve(timeout)
if not solved:
self._log("Could not find plan")
return None
objective = pdef.getOptimizationObjective()
if objective is not None:
cost = (pdef.getSolutionPath().cost(
pdef.getOptimizationObjective()).value())
else:
cost = np.inf
self._log("Got a path of length %.2f and cost %.2f" %
(pdef.getSolutionPath().length(), cost))
path = pdef.getSolutionPath()
joint_plan = mp_space.convert_sol(path)
self._is_approx_sol = pdef.hasApproximateSolution()
return joint_plan
def plan(runTime, plannerType, objectiveType, fname, bound, start_pt, goal_pt):
print "Run Time: ", runTime
# 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(bound[0], bound[1])
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = start_pt[0]
start[1] = start_pt[1]
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = goal_pt[0]
goal[1] = goal_pt[1]
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
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(optimizingPlanner.getName(), pdef.getSolutionPath().length(), pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname,'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
return True, pdef.getSolutionPath().printAsMatrix()
else:
print("No solution found.")
return False, "No solution found"
def problem_definition(self, space, space_info, start_pos, goal_pos):
"""
Function which define path problem that includes start and goal states
between which the path must be built
"""
pdef = ob.ProblemDefinition(space_info)
start = ob.State(space)
start[0] = start_pos[0]
start[1] = start_pos[1]
goal = ob.State(space)
goal[0] = goal_pos[0]
goal[1] = goal_pos[1]
pdef.setStartAndGoalStates(start, goal)
return pdef
def planTheHardWay():
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# construct an instance of space information from this state space
si = ob.SpaceInformation(space)
# set state validity checking for this space
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create a random start state
start = ob.State(space)
start.random()
# create a random goal state
goal = ob.State(space)
goal.random()
# create a problem instance
pdef = ob.ProblemDefinition(si)
# set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.RRTConnect(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print(si.settings())
# print the problem settings
print(pdef)
# attempt to solve the problem within one second of planning time
solved = planner.solve(1.0)
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n%s" % path)
else:
print("No solution found")
def test_rigid(calc_time=0.01):
space = ob.RealVectorStateSpace(2)
space.setBounds(0, WIDTH)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ValidityChecker(si))
si.setup()
start = ob.State(space)
goal = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal[0] = WIDTH - 1
goal[1] = WIDTH - 1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
planner = og.RRTstar(si)
planner.setProblemDefinition(pdef)
planner.setup()
status = planner.solve(calc_time)
if pdef.hasExactSolution():
cost = pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()
print(cost)
def plan(self, start_row, start_col, goal_row, goal_col, plannerType,
runtime):
if not self.si:
return False
start = ob.State(self.space)
start()[0] = start_row
start()[1] = start_col
goal = ob.State(self.space)
goal()[0] = goal_row
goal()[1] = goal_col
# Create a problem instance
self.pdef = ob.ProblemDefinition(self.si)
# Set the start and goal states
self.pdef.setStartAndGoalStates(start, goal)
self.planner = self.allocatePlanner(self.si, plannerType)
self.planner.setProblemDefinition(self.pdef)
# perform setup steps for the planner
self.planner.setup()
# print the settings for this space
print(self.si.settings())
# generate a few solutions; all will be added to the goal
for i in range(5):
self.solved = self.planner.solve(runtime)
p = self.pdef.getSolutionPath()
print("Found solution:\n%s" % p)
ns = self.planner.getProblemDefinition().getSolutionCount()
print("Found %d solutions" % ns)
if self.solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = self.pdef.getSolutionPath()
print("Found solution:\n%s" % path)
return True
else:
print("No solution found")
return False
def plan(runTime, plannerType, objectiveType, fname):
# Construct the robot state space in which we're planning. We're
# planning in [0,500]x[0,500], a subset of R^2.
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,500].
space.setBounds(0.0, 500.0)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be random
start = ob.State(space)
start[0] = random.randint(0,500)
start[1] = random.randint(0,500)
while (sqrt((start[0]-center[0])*(start[0]-center[0]) + (start[1]-center[0])*(start[1]-center[0])) - radius < 0):
start[0] = random.randint(0,500)
start[1] = random.randint(0,500)
# Set our robot's goal state to be random
goal = ob.State(space)
goal[0] = random.randint(0,500)
goal[1] = random.randint(0,500)
while (sqrt((goal[0]-center[0])*(goal[0]-center[0]) + (goal[1]-center[0])*(goal[1]-center[0])) - radius < 0):
goal[0] = random.randint(0,500)
goal[1] = random.randint(0,500)
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
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( \
optimizingPlanner.getName(), \
pdef.getSolutionPath().length(), \
pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
else:
print("No solution found.")
def plan(grid):
#agent and goal are represented by a point(x,y) and radius
global x
global time
x = grid
agent: Agent = grid.agent
goal: Goal = grid.goal
# Construct the robot state space in which we're planning. R2
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be inside Map
bounds = ob.RealVectorBounds(2)
# projection
pj = ProjectionEvaluator(space)
print('pj=', pj)
pj.setCellSizes(list2vec([1.0, 1.0]))
space.registerDefaultProjection(pj)
# Construct the robot state space in which we're planning.
bounds.setLow(0, 0) #set min x to _ 0
bounds.setHigh(0, grid.size.width) #set max x to _ x width
bounds.setLow(1, 0) #set min y to _ 0
bounds.setHigh(1, grid.size.height) #set max y to _ y height
space.setBounds(bounds)
print("bounds=", bounds.getVolume())
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# Set robot's starting state to agent's position (x,y) -> e.g. (0,0)
start = ob.State(space)
start[0] = float(agent.position.x)
start[1] = float(agent.position.y)
print(start[0], start[1])
# Set robot's goal state (x,y) -> e.g. (1.0,0.0)
goal = ob.State(space)
goal[0] = float(grid.goal.position.x)
goal[1] = float(grid.goal.position.y)
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
#pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# ******create a planner for the defined space
planner = og.RRTXstatic(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
print(planner)
#print('checking projection',planner.getProjectionEvaluator())
print("__________________________________________________________")
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print("space settings\n")
print(si.settings())
print("****************************************************************")
print("problem settings\n")
# print the problem settings
print(pdef)
# attempt to solve the problem within ten second of planning time
solved = planner.solve(time)
# For troubleshooting
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n%s" % path)
#return trace for _find_path_internal method
print(
"$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"
)
if path is None:
return None
x = pdef.getSolutionPath().printAsMatrix()
lst = x.split()
lst = [int(round(float(x), 0)) for x in lst]
print(x)
print(lst)
trace = []
for i in range(0, len(lst), 2):
trace.append(Point(lst[i], lst[i + 1]))
print(trace)
return trace
else:
print("No solution found")
return None
def plan(self, runTime, plannerType, objectiveType, fname):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
self.space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
self.space.setBounds(0, 400.0)
# Construct a space information instance for this state space
self.si = ob.SpaceInformation(self.space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(self.si, self.mapIm)
self.si.setStateValidityChecker(validityChecker)
self.si.setup()
self.start = ob.State(self.space)
self.start[0] = self.startPose[0]
self.start[1] = self.startPose[1]
self.goal = ob.State(self.space)
self.goal[0] = self.goalPose[0]
self.goal[1] = self.goalPose[1]
# Create a problem instance
self.pdef = ob.ProblemDefinition(self.si)
# Set the start and goal states
self.pdef.setStartAndGoalStates(self.start, self.goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
self.pdef.setOptimizationObjective(
self.allocateObjective(self.si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
self.optimizingPlanner = self.allocatePlanner(self.si, plannerType)
# Set the problem instance for our planner to solve
self.optimizingPlanner.setProblemDefinition(self.pdef)
self.optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = self.optimizingPlanner.solve(runTime)
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(
self.optimizingPlanner.getName(),
self.pdef.getSolutionPath().length(),
self.pdef.getSolutionPath().cost(
self.pdef.getOptimizationObjective()).value()))
print self.pdef.getSolutionPath()
# If a filename was specified, output the path as a matrix to
# that file for visualization
# if fname:
# with open(fname,'w') as outFile:
# outFile.write(pdef.getSolutionPath().printAsMatrix())
else:
print("No solution found.")
def plan(self, controlled_object, x1, y1, v1, a1):
a1 = (a1 + 2 * np.pi) % (2 * np.pi)
car = deepcopy(controlled_object)
space = ob.RealVectorStateSpace(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(0, 0)
bounds.setLow(1, 0)
bounds.setLow(2, 0)
bounds.setHigh(0, 1000)
bounds.setHigh(1, 1000)
bounds.setHigh(2, 2 * np.pi)
space.setBounds(bounds)
si = ob.SpaceInformation(space)
validityChecker = ValidityChecker(si, self.state, car)
si.setStateValidityChecker(validityChecker)
#si.setMotionValidator(MotionValidityChecker(si, state, agent_num))
si.setup()
start = ob.State(space)
start[0] = car.x
start[1] = car.y
start[2] = car.angle
goal = ob.State(space)
goal[0] = x1
goal[1] = y1
goal[2] = a1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(si))
def objStateCost(state):
return ob.Cost(0)
def objMotionCost(state1, state2):
x, y, a = state1[0], state1[1], state1[2]
xt, yt, at = state2[0], state2[1], state2[2]
yd, xd = yt - y, xt - x
ax = np.arctan(yd / (xd + 0.001))
ax = ax % (np.pi)
if yd < 0:
ax = ax + np.pi
ad = min(np.abs(at - ax), np.abs((ax - at)))
return 10 * ad
pathObj = ob.OptimizationObjective(si)
pathObj.stateCost = objStateCost
pathObj.motionCost = objMotionCost
pathObj.setCostThreshold(1)
#pdef.setOptimizationObjective(pathObj)
optimizingPlanner = og.RRTstar(si)
optimizingPlanner.setRange(self.inter_point_d)
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = optimizingPlanner.solve(self.planning_time)
sol = pdef.getSolutionPath()
if not sol:
return []
sol = sol.printAsMatrix()
s = [[float(j) for j in i.split(" ")[:-1]]
for i in sol.splitlines()][:-1]
v0 = car.vel
num_points = len(s)
for i in range(num_points):
[i].append(v0 * (1 - float(i) / float(num_points)) + v1 *
(float(i) / float(num_points)))
return s
si_R2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_R2))
# Create vector of spaceinformationptr
si_vec = og.vectorSpaceInformation()
si_vec.append(si_R2)
si_vec.append(si_SE2)
# Define Planning Problem
start_SE2 = ob.State(SE2)
goal_SE2 = ob.State(SE2)
start_SE2().setXY(0, 0)
start_SE2().setYaw(0)
goal_SE2().setXY(1, 1)
goal_SE2().setYaw(0)
pdef = ob.ProblemDefinition(si_SE2)
pdef.setStartAndGoalStates(start_SE2, goal_SE2)
# Setup Planner using vector of spaceinformationptr
planner = og.QRRT(si_vec)
# Planner can be used as any other OMPL algorithm
planner.setProblemDefinition(pdef)
planner.setup()
solved = planner.solve(1.0)
if solved:
print('-' * 80)
print('Configuration-Space Path (SE2):')
print('-' * 80)
def plan(self, start_tup, goal_tup, turning_radius=10, planning_time=30.0):
def isStateValid(state):
y = int(state.getY())
x = int(state.getX())
if y < 0 or x < 0 or y >= self.map_data.shape[
0] or x >= self.map_data.shape[1]:
return False
return bool(self.map_data[y, x] == 0)
space = ob.DubinsStateSpace(turningRadius=turning_radius)
# Set State Space bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, 0)
bounds.setHigh(0, self.map_data.shape[1])
bounds.setLow(1, 0)
bounds.setHigh(1, self.map_data.shape[0])
space.setBounds(bounds)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setStateValidityCheckingResolution(
self.params["validity_resolution"]
) # Set based on thinness of walls in map
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(
ob.ObstacleBasedValidStateSampler(si)))
si.setup()
# Set Start and Goal states
start = ob.State(space)
start().setX(start_tup[0])
start().setY(start_tup[1])
start().setYaw(start_tup[2])
goal = ob.State(space)
goal().setX(goal_tup[0])
goal().setY(goal_tup[1])
goal().setYaw(goal_tup[2])
# Set Problem Definition
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
optimObj = ob.PathLengthOptimizationObjective(si)
pdef.setOptimizationObjective(optimObj)
# Set up planner
optimizingPlanner = og.BITstar(si)
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = optimizingPlanner.solve(planning_time)
def solution_path_to_tup(solution_path):
result = []
states = solution_path.getStates()
for state in states:
x = state.getX()
y = state.getY()
yaw = state.getYaw()
result.append((x, y, yaw))
return result
solutionPath = None
if solved:
solutionPath = pdef.getSolutionPath()
solutionPath.interpolate(self.params['interpolation_density'])
solutionPath = solution_path_to_tup(solutionPath)
return bool(solved), solutionPath
def plan_trajectory(self,
start_point,
goal_point,
planner_number,
joint_names,
group_name,
planning_time,
planner_config_name,
plan_type='pfs'):
"""
Use OMPL library for planning. Obtain obstacle information from rostopic for
collision checking
"""
# obtain obstacle information through rostopic
rospy.loginfo(
"%s Plan Trajectory Wrapper: waiting for obstacle message..." %
(rospy.get_name()))
obc = rospy.wait_for_message('obstacles/obc', Float64Array2D)
# obs = rospy.wait_for_message('obstacles/obs', Float64Array2D)
obc = [obc_i.values for obc_i in obc.points]
obc = np.array(obc)
rospy.loginfo(
"%s Plan Trajectory Wrapper: obstacle message received." %
(rospy.get_name()))
# depending on plan type, obtain path_length from published topic or not
if plan_type == 'pfs':
# obtain path length through rostopic
rospy.loginfo(
"%s Plan Trajectory Wrapper: waiting for planning path length message..."
% (rospy.get_name()))
path_length = rospy.wait_for_message(
'planning/path_length_threshold', Float64)
path_length = path_length.data
rospy.loginfo(
"%s Plan Trajectory Wrapper: planning path length received." %
(rospy.get_name()))
elif plan_type == 'rr':
path_length = np.inf # set a very large path length because we only want feasible paths
# reshape
# plan
IsInCollision = self.IsInCollision
rospy.loginfo("%s Plan Trajectory Wrapper: start planning..." %
(rospy.get_name()))
# create a simple setup object
start = ob.State(self.space)
# we can pick a random start state...
# ... or set specific values
for k in xrange(len(start_point)):
start[k] = start_point[k]
goal = ob.State(self.space)
for k in xrange(len(goal_point)):
goal[k] = goal_point[k]
def isStateValid(state):
return not IsInCollision(state, obc)
si = ob.SpaceInformation(self.space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setup()
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
pdef.setOptimizationObjective(getPathLengthObjective(si, path_length))
ss = allocatePlanner(si, self.planner_name)
ss.setProblemDefinition(pdef)
ss.setup()
plan_time = time.time()
solved = ss.solve(planning_time)
plan_time = time.time() - plan_time
if solved:
rospy.loginfo(
"%s Plan Trajectory Wrapper: OMPL Planner solved successfully."
% (rospy.get_name()))
# obtain planned path
ompl_path = pdef.getSolutionPath().getStates()
solutions = np.zeros((len(ompl_path), 2))
for k in xrange(len(ompl_path)):
solutions[k][0] = float(ompl_path[k][0])
solutions[k][1] = float(ompl_path[k][1])
return plan_time, solutions.tolist()
else:
return np.inf, None
def plan(self, runTime, plannerType, objectiveType, fname):
# 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(0.0, 1.0)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = self.start_x
start[1] = self.start_y
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = self.goal_x
goal[1] = self.goal_y
print("goal")
print self.goal_x
print self.goal_y
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
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(optimizingPlanner.getName(), pdef.getSolutionPath().length(), pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
str = pdef.getSolutionPath().printAsMatrix()
#print(type(str))
#
x = str.split()
self.path = [float(x) for x in x if x]
#print(self.path)
#talker()
else:
print("No solution found.")
return self.path
def plan(fname=None):
# 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(0.0, 1.0)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = 0.0
start[1] = 0.0
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = 1.0
goal[1] = 1.0
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Since we want to find an optimal plan, we need to define what
# is optimal with an OptimizationObjective structure. Un-comment
# exactly one of the following 6 lines to see some examples of
# optimization objectives.
pdef.setOptimizationObjective(getPathLengthObjective(si))
# pdef.setOptimizationObjective(getThresholdPathLengthObj(si))
# pdef.setOptimizationObjective(getClearanceObjective(si))
# pdef.setOptimizationObjective(getBalancedObjective1(si))
# pdef.setOptimizationObjective(getBalancedObjective2(si))
# pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si))
# Construct our optimal planner using the RRTstar algorithm.
optimizingPlanner = og.RRTstar(si)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem within one second of
# planning time
solved = optimizingPlanner.solve(10.0)
if solved:
# Output the length of the path found
print("Found solution of path length %g" %
pdef.getSolutionPath().length())
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
else:
print("No solution found.")
def plan(mapfile,start_node, goal_node,runTime, plannerType, objectiveType, d, fname):
boundary, blocks = load_map(mapfile)
boundary[:,:3] = boundary[:,:3] +0.0005
boundary[:,3:] = boundary[:,3:] -0.0005
alpha = 1.05
blocks = blocks*alpha
# Construct the robot state space in which we're planning. We're
space = ob.RealVectorStateSpace(3)
sbounds = ob.RealVectorBounds(3)
sbounds.low[0] =float(boundary[0,0])
sbounds.high[0] =float(boundary[0,3])
sbounds.low[1] = float(boundary[0,1])
sbounds.high[1] = float(boundary[0,4])
sbounds.low[2] = float(boundary[0,2])
sbounds.high[2] = float(boundary[0,5])
space.setBounds(sbounds)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si, boundary, blocks)
si.setStateValidityChecker(validityChecker)
mv = MyMotionValidator(si, boundary, blocks)
si.setMotionValidator(mv)
si.setup()
# Set our robot's starting state
start = ob.State(space)
start[0] = start_node[0]
start[1] = start_node[1]
start[2] = start_node[2]
# Set our robot's goal state
goal = ob.State(space)
goal[0] = goal_node[0]
goal[1] = goal_node[1]
goal[2] = goal_node[2]
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si,d, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = "Approximate solution"
# attempt to solve the planning problem in the given runtime
t1 = tic()
while(str(solved) == 'Approximate solution'):
solved = optimizingPlanner.solve(runTime)
# print(pdef.getSolutionPath().printAsMatrix())
t2 = toc(t1)
p = pdef.getSolutionPath()
ps = og.PathSimplifier(si)
ps.simplifyMax(p)
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( \
optimizingPlanner.getName(), \
pdef.getSolutionPath().length(), \
pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
# If a filename was specified, output the path as a matrix to
# that file for visualization
env = mapfile.split(".")[1].split("/")[-1]
fname = fname + "{}_{}_{}_{}.txt".format(env, d,np.round(pdef.getSolutionPath().length(),2),np.round(t2,2))
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
path = np.genfromtxt(fname)
print("The found path : ")
print(path)
pathlength = np.sum(np.sqrt(np.sum(np.diff(path,axis=0)**2,axis=1)))
print("pathlength : {}".format(pathlength))
fig, ax, hb, hs, hg = draw_map(boundary, blocks, start_node, goal_node)
ax.plot(path[:,0],path[:,1],path[:,2],'r-')
plt.show()
def plan(runTime, plannerType, objectiveType, fname):
# 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()
# Set the bounds of space to be in [0,1]
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(1)
space.setBounds(bounds)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = 0
start[1] = 0
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = 1
goal[1] = 1
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# Construct the optimal planner specified by us.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem we are trying to solve to the planner
optimizingPlanner.setProblemDefinition(pdef)
#perform setup steps for the planner
optimizingPlanner.setup()
#print the settings for this space
#print(si.settings())
#print the problem settings
#print(pdef)
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
if solved:
#print solution path
path = pdef.getSolutionPath()
print("Found Solution:\n%s" % path)
# 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( \
optimizingPlanner.getName(), \
pdef.getSolutionPath().length(), \
pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
print("saved final path as 'mat.txt' for matplotlib")
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(pdef.getSpaceInformation())
optimizingPlanner.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
#dataStorage=ob.PlannerDataStorage()
#dataStorage.store(pd,"myPlannerData")
graphml = pd.printGraphML()
f = open("graph.graphml", 'w')
f.write(graphml)
f.close()
print("saved")
else:
print("No solution found.")
def OMPL_plan(self, runTime, plannerType, objectiveType, mult):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
if self.dimension == '2D':
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
bounds = ob.RealVectorBounds(2)
x_bound = []
y_bound = []
for idx in range(len(self.region)):
x_bound.append(self.region[idx][0])
y_bound.append(self.region[idx][1])
bounds.setLow(0, min(x_bound))
bounds.setHigh(0, max(x_bound))
bounds.setLow(1, min(y_bound))
bounds.setHigh(1, max(y_bound))
self.x_bound = (min(x_bound), max(x_bound))
self.y_bound = (min(y_bound), max(y_bound))
#test = bounds.getDifference()
#test = bounds.check()
space.setBounds(bounds)
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = self.start[0]
start[1] = self.start[1]
#start[2] = 0.0
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = self.goal[0]
goal[1] = self.goal[1]
#goal[2] = 1.0
else:
pass
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = self.ValidityChecker(si)
validityChecker.setInteractive()
validityChecker.obstacle(self.start, self.goal, self.region,
self.obstacle, self.dimension)
si.setStateValidityChecker(validityChecker)
si.setup()
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(self.allocateObjective(
si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = self.allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setRewireFactor(1.1)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
self.PlannerStates.append(
(validityChecker.getInteractive(), plannerType))
if solved:
# Output the length of the path found
try:
print('{0} found solution of path length {1:.4f} with an optimization ' \
'objective value of {2:.4f}'.format( \
optimizingPlanner.getName(), \
pdef.getSolutionPath().length(), \
pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
return self.decodeSolutionPath(
pdef.getSolutionPath().printAsMatrix(), plannerType,
pdef.getSolutionPath().cost(
pdef.getOptimizationObjective()).value(), mult)
except:
print("No solution found.")
pass
else:
print("No solution found.")
