def planning(self, runtime=50.0):
if self.start is None and self.goal is None:
return False
self.ss.clear()
dt = 5.0
tic = time.time()
find_exact_solution = False
self.ss.setStartAndGoalStates(self.start, self.goal, 1.0)
for _ in np.arange(0.0, runtime+dt, dt):
self.ss.solve(dt)
path_matrix = self.ss.getSolutionPath().printAsMatrix()
path = np.array([j.split()
for j in path_matrix.splitlines()], dtype=float)
if self.check_reach(path):
find_exact_solution = True
break
toc = time.time()
self.planning_time = toc - tic
self.path = path
# get planner data
pd = ob.PlannerData(self.ss.getSpaceInformation())
self.ss.getPlannerData(pd)
pd.computeEdgeWeights()
xmlstring = pd.printGraphML()
self.planner_data = self.xml2tree(xmlstring)
self.reach_exactly = find_exact_solution
return find_exact_solution
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, -40)
bounds.setHigh(0, 40)
bounds.setLow(1, 0)
bounds.setHigh(1, 90)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(0)
start().setY(0)
start().setYaw(0)
# start().setZ(-9)
# start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-25)
goal().setY(60)
goal().setYaw(0)
# goal().setZ(-9)
# goal().rotation().setIdentity()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# Lets use PRM. It will have interesting PlannerData
planner = og.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
# To change time change value given to ss.solve
solved = ss.solve(1.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
path = get_path(ss)
draw_graph(path)
plt.show()
def plan(runTime, plannerType, objectiveType, plotData, fname, pdfname):
space = ob.RealVectorStateSpace(2)
space.setBounds(0.0, 1.0)
ss = og.SimpleSetup(space)
start = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal = ob.State(space)
goal[0] = 1.0
goal[1] = 1.0
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
ss.setStartAndGoalStates(start, goal)
si = ss.getSpaceInformation()
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
ss.setOptimizationObjective(allocateObjective(si, objectiveType))
ss.setPlanner(allocatePlanner(si, plannerType))
ss.setup()
# attempt to solve the problem
solved = ss.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( \
ss.getSolutionPlannerName(), \
ss.getSolutionPath().length(), \
ss.getSolutionPath().cost(ss.getOptimizationObjective()).value()))
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(si)
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
if graphtool and plotData:
useGraphTool(pd)
# 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(ss.getSolutionPath().printAsMatrix())
if pdfname:
pds = ob.PlannerDataStorage()
pds.store(pd, pdfname)
else:
print("No solution found.")
def plan():
# create an Vector state space
space = ob.RealVectorStateSpace(SPACE_DIM)
# # set lower and upper bounds
bounds = ob.RealVectorBounds(SPACE_DIM)
for i in range(SPACE_DIM - 3):
bounds.setLow(i, min_bound)
bounds.setHigh(i, max_bound)
for i in range(SPACE_DIM - 3):
bounds.setLow(i + 3, -np.pi)
bounds.setHigh(i + 3, np.pi)
space.setBounds(bounds)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
# set planner
planner = og.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# start.random()
start_state = start.get()
for i in range(SPACE_DIM):
start_state[i] = start_s[i]
# print(start_state[i])
goal = ob.State(space)
# goal.random()
goal_state = goal.get()
for i in range(SPACE_DIM):
goal_state[i] = goal_s[i]
# print(goal_state[i])
ss.setStartAndGoalStates(start, goal)
# default parameters
solved = ss.solve(4.0)
if solved:
# パスの簡単化を実施
ss.simplifySolution()
# 結果を取得
path_result = ss.getSolutionPath()
print(path_result)
si = ss.getSpaceInformation()
pdata = ob.PlannerData(si)
ss.getPlannerData(pdata)
space = path_result.getSpaceInformation().getStateSpace()
plot_result(space, path_result, pdata)
def dumpGraph(self, name):
ou.OMPL_INFORM("Dumping planner graph to `%s_graph.graphml`." % name)
data = ob.PlannerData(self.csi)
self.pp.getPlannerData(data)
with open("%s_graph.graphml" % name, "w") as graphfile:
print(data.printGraphML(), file=graphfile)
if self.spaceType == "AT" or self.spaceType == "TB":
ou.OMPL_INFORM("Dumping atlas to `%s_atlas.ply`." % name)
with open("%s_atlas.ply" % name, "w") as atlasfile:
print(self.css.printPLY(), file=atlasfile)
def plan():
# construct the state space we are planning in
space = ob.SE3StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(-10)
bounds.setHigh(10)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(-9)
start().setY(-9)
start().setZ(-9)
start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-9)
goal().setY(9)
goal().setZ(-9)
goal().rotation().setIdentity()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# Lets use PRM. It will have interesting PlannerData
planner = og.PRM(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
if graphtool:
useGraphTool(pd)
def plt_ompl_result(condition, start, goal, path, collisionchecker, spaceinfo, planner):
fig1 = plt.figure(figsize=(10, 6), dpi=80)
ax1 = fig1.add_subplot(111, aspect='equal')
obs, dimW = gap2obs(condition)
for i in range(0, obs.shape[0] // (2 * dimW)): # plot obstacle patches
ax1.add_patch(
patches.Rectangle(
(obs[i * 2 * dimW], obs[i * 2 * dimW + 1]), # (x,y)
obs[i * 2 * dimW + dimW] - obs[i * 2 * dimW], # width
obs[i * 2 * dimW + dimW + 1] - obs[i * 2 * dimW + 1], # height
alpha=0.6
))
gridSize = 11
plannerdata = ob.PlannerData(spaceinfo)
planner.getPlannerData(plannerdata)
print("num edges: ", plannerdata.numEdges())
print("num vertices: ", plannerdata.numVertices())
num_ver = plannerdata.numVertices()
num_edge = plannerdata.numEdges()
if num_edge > 3000:
print("too much edges")
edge_vis = False
else:
edge_vis = True
for i in range(0, num_ver):
plt.scatter( plannerdata.getVertex(i).getState()[0], plannerdata.getVertex(i).getState()[1], color="green", s=50, edgecolors='black') # vertice
if edge_vis:
for j in range(0, num_ver):
if plannerdata.edgeExists(i, j):
plt.plot([plannerdata.getVertex(i).getState()[0], plannerdata.getVertex(j).getState()[0]], [plannerdata.getVertex(i).getState()[1], plannerdata.getVertex(j).getState()[1]], color='gray')
path.interpolate()
states = path.getStates()
for state in states:
plt.scatter(state[0], state[1], color="green", s=250, edgecolors='black') # path
# for state in collisionchecker.states_ok:
# plt.scatter(state[0], state[1], color="green", s=100, edgecolors='green') # free sample
# for state in collisionchecker.states_bad:
# plt.scatter(state[0], state[1], color="red", s=100, edgecolors='red') # collision sample
plt.scatter(start()[0], start()[1], color="blue", s=250, edgecolors='black') # init
plt.scatter(goal()[0], goal()[1], color="red", s=250, edgecolors='black') # goal
plt.xlabel('x')
plt.ylabel('y')
plt.show()
def graph(self, with_p=False):
planner = self.ss.getPlanner()
pd = ob.PlannerData(self.ss.getSpaceInformation())
planner.getPlannerData(pd)
g = nx.read_graphml(io.StringIO(pd.printGraphML()))
traversable = self.map.traversable
for n, data in g.nodes(data=True):
x = tuple(map(float, data['coords'].split(',')[:3]))
data['pos'] = x
for n1, n2, data in list(g.edges(data=True)):
x = g.node[n1]['pos']
y = g.node[n2]['pos']
v, omega = control(x, y)
data['v'] = v
data['omega'] = omega
if with_p:
data['p'], _ = traversable(x, (v, omega))
return g
def graph(self):
ros = self.map.to_ros
fros = self.map.from_ros
planner = self.ss.getPlanner()
pd = ob.PlannerData(self.ss.getSpaceInformation())
planner.getPlannerData(pd)
g = nx.read_graphml(io.StringIO(pd.printGraphML()))
for n, data in g.nodes(data=True):
x = list(map(float, data['coords'].split(',')[:2]))
data['pos'] = x
data['e_pos'] = ros(*x, with_z=True)
traversable = self.map.traversable
for s, t, data in g.edges(data=True):
sx, sy = g.node[s]['pos']
tx, ty = g.node[t]['pos']
p, d = traversable((sx, sy, self.theta), (tx, ty), frame='abs')
data['probability'] = p
data['duration'] = d
return g
si = ss.getSpaceInformation()
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
planner = og.RRTstar(si)
planner = og.PRM(si)
ss.setPlanner(planner)
solved = ss.solve(0.05)
c.draw()
if solved:
ss.simplifySolution()
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
pd.computeEdgeWeights()
Vn = pd.numVertices()
for n in range(0, Vn):
v = pd.getVertex(n)
drawVertex(v)
Ve = pd.numEdges()
for i in range(0, Vn):
for j in range(0, Vn):
if pd.edgeExists(i, j):
v1 = pd.getVertex(i)
v2 = pd.getVertex(j)
drawEdge(v1, v2)
def solve(problem):
"""
Given an instance of the Problem class, containing the problem configuration
data, solves the motion planning problem and returns either the solution
path or a failure message.
"""
# Sets up the robot type related information
ompl_setup = setup(problem)
# Load the planner
space_info = ompl_setup.getSpaceInformation()
if problem['planner'].startswith('ompl.control.Syclop'):
decomposition = ompl_setup.allocDecomposition()
planner = eval('%s(space_info, decomposition)' % problem['planner'])
ompl_setup.setPlanner(planner)
else:
planner = eval("%s(space_info)" % problem['planner'])
ompl_setup.setPlanner(planner)
# Set the optimization objective
objectives = {'length': 'PathLengthOptimizationObjective', \
'max_min_clearance': 'MaximizeMinClearanceObjective', \
'mechanical_work': 'MechanicalWorkOptimizationObjective'}
objective = objectives[problem['objective']]
obj = eval('ob.%s(space_info)' % objective)
cost = ob.Cost(float(problem['objective.threshold']))
obj.setCostThreshold(cost)
ompl_setup.setOptimizationObjective(obj)
# Set each parameter that was configured by the user
for param in problem['planner_params']:
planner.params().setParam(str(param), str(problem['planner_params'][param]))
# Solve the problem
solution = {}
solved = ompl_setup.solve(float(problem['solve_time']))
# Check for validity
if solved:
if problem['isGeometric']:
path = ompl_setup.getSolutionPath()
initialValid = path.check()
if initialValid:
# If initially valid, attempt to simplify
ompl_setup.simplifySolution()
# Get the simplified path
simple_path = ompl_setup.getSolutionPath()
simplifyValid = simple_path.check()
if simplifyValid:
path = simple_path
else:
OMPL_ERROR("Simplified path was invalid.")
else:
OMPL_ERROR("Invalid solution path.")
else:
path = ompl_setup.getSolutionPath().asGeometric()
OMPL_INFORM("Path simplification skipped due to non rigid body.")
# Interpolate path
ns = int(100.0 * float(path.length()) / \
float(ompl_setup.getStateSpace().getMaximumExtent()))
if problem["isGeometric"] and len(path.getStates()) < ns:
path.interpolate(ns)
if len(path.getStates()) != ns:
OMPL_WARN("Interpolation produced " + str(len(path.getStates())) + \
" states instead of " + str(ns) + " states.")
solution = format_solution(path, True)
else:
solution = format_solution(None, False)
solution['name'] = str(problem['name'])
solution['planner'] = ompl_setup.getPlanner().getName()
solution['status'] = str(solved)
# Store the planner data
pd = ob.PlannerData(ompl_setup.getSpaceInformation())
ompl_setup.getPlannerData(pd)
explored_states = []
for i in range(0, pd.numVertices()):
coords = []
state = pd.getVertex(i).getState()
coords.append(state.getX())
coords.append(state.getY())
if problem["is3D"]:
coords.append(state.getZ())
else:
coords.append(0)
explored_states.insert(i, coords)
solution["explored_states"] = explored_states
return solution
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 plan(self, planning_query: PlanningQuery):
self.cleanup_before_plan(planning_query.seed)
self.environment = planning_query.environment
self.goal_region = self.scenario_ompl.make_goal_region(
self.si,
rng=self.goal_sampler_rng,
params=self.params,
goal=planning_query.goal,
plot=self.verbose >= 2)
# create start and goal states
start_state = planning_query.start
start_state['stdev'] = np.array([0.0])
start_state['num_diverged'] = np.array([0.0])
self.start_state = start_state
ompl_start_scoped = ob.State(self.state_space)
self.scenario_ompl.numpy_to_ompl_state(start_state,
ompl_start_scoped())
# visualization
self.scenario.reset_planning_viz()
self.scenario.plot_environment_rviz(planning_query.environment)
self.scenario.plot_start_state(start_state)
self.scenario.plot_goal_rviz(planning_query.goal,
self.params['goal_params']['threshold'])
self.ss.clear()
self.ss.setStartState(ompl_start_scoped)
self.ss.setGoal(self.goal_region)
self.ptc = TimeoutOrNotProgressing(self,
self.params['termination_criteria'],
self.verbose)
# START TIMING
t0 = time.time()
# acutally run the planner
ob_planner_status = self.ss.solve(self.ptc)
# END TIMING
planning_time = time.time() - t0
# handle results and cleanup
planner_status = interpret_planner_status(ob_planner_status, self.ptc)
if planner_status == MyPlannerStatus.Solved:
ompl_path = self.ss.getSolutionPath()
actions, planned_path = self.convert_path(ompl_path)
planner_data = ob.PlannerData(self.si)
self.rrt.getPlannerData(planner_data)
tree = planner_data_to_json(planner_data, self.scenario_ompl)
elif planner_status == MyPlannerStatus.Timeout:
# Use the approximate solution, since it's usually pretty darn close, and sometimes
# our goals are impossible to reach so this is important to have
try:
ompl_path = self.ss.getSolutionPath()
actions, planned_path = self.convert_path(ompl_path)
except RuntimeError:
rospy.logerr(
"Timeout before any edges were added. Considering this as Not Progressing."
)
planner_status = MyPlannerStatus.NotProgressing
actions = []
tree = {}
planned_path = [start_state]
else: # if no exception was raised
planner_data = ob.PlannerData(self.si)
self.rrt.getPlannerData(planner_data)
tree = planner_data_to_json(planner_data, self.scenario_ompl)
elif planner_status == MyPlannerStatus.Failure:
rospy.logerr(f"Failed at starting state: {start_state}")
tree = {}
actions = []
planned_path = [start_state]
elif planner_status == MyPlannerStatus.NotProgressing:
tree = {}
actions = []
planned_path = [start_state]
print()
return PlanningResult(status=planner_status,
path=planned_path,
actions=actions,
time=planning_time,
tree=tree)
pcp.set_alpha(0.7)
pcc.set_linestyle('dashed')
pcp.set_linestyle('dashed')
pcc.set_edgecolor('#73cdc9')
pcp.set_edgecolor('#73cdc9')
pcc.set_linewidth(0.8)
pcp.set_linewidth(0.8)
pcc.set_joinstyle('round')
pcp.set_joinstyle('round')
else:
pcc.set_color('gray')
pcp.set_color('gray')
ax.add_collection(pcc)
ax.add_collection(pcp)
if args.plannerdata:
pd = ob.PlannerData(ob.SpaceInformation(ob.RealVectorStateSpace(2)))
pds = ob.PlannerDataStorage()
pds.load(args.plannerdata, pd)
pd.computeEdgeWeights()
# Extract the graphml representation of the planner data
graphml = pd.printGraphML()
f = open("graph.graphml", 'w')
f.write(graphml)
f.close()
# Load the graphml data using graph-tool
graph = gt.load_graph("graph.graphml", fmt="xml")
os.remove("graph.graphml")
edgeweights = graph.edge_properties["weight"]