class CSpaceObstacleProgram(GLProgram):
def __init__(self, space, start, goal, initial_points=1000):
GLProgram.__init__(self)
self.space = space
#PRM planner
MotionPlan.setOptions(
type="prm*", knn=10, connectionThreshold=0.2
) # Change type based on what planner you want to use
self.optimizingPlanner = True
self.planner = MotionPlan(space)
self.start = start
self.goal = goal
self.planner.addMilestone(start)
self.planner.addMilestone(goal)
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
self.path = []
self.G = None
class CSpaceObstacleSolver:
def __init__(self,
space,
x_bound=1.0,
y_bound=1.0,
milestones=(0, 0),
initial_points=4000):
self.space = space
#PRM planner
MotionPlan.setOptions(type="prm",
knn=10,
connectionThreshold=0.05,
ignoreConnectedComponents=True)
self.optimizingPlanner = False
# we first plan a bit without goals just to have a good skeleton
self.initial_points = initial_points
self.x_bound = x_bound
self.y_bound = y_bound
self.milestones = milestones
self.times = 0
self.planner = MotionPlan(space)
print('planning initial roadmap with {} points'.format(initial_points))
self.planner.planMore(self.initial_points)
self.connected = False
# we now add each of the chosen points as a milestone:
self.G = self.planner.getRoadmap()
self.start_milestones = len(self.G[0])
for milestone in self.milestones:
self.planner.addMilestone(milestone)
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
self.path = []
self.G = None
def get_adjacency_matrix_from_milestones(self):
while (self.components > 1):
print("Planning 100...")
self.planner.planMore(100)
self.path = self.planner.getPath()
self.G = self.planner.getRoadmap()
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
print(
'PRM connecting all milestones found - computing adjacency matrix')
pathDict = dict()
self.adjacency_matrix = np.zeros(shape=(len(self.milestones),
len(self.milestones)))
self.adjacency_matrix[:, :] = np.inf
for i, milestone1 in tqdm(
enumerate(
range(self.start_milestones,
self.start_milestones + 1 + len(self.milestones)))):
# print(self.G[0][milestone1])
for j, milestone2 in enumerate(
range(milestone1 + 1,
self.start_milestones + len(self.milestones))):
j = j + i + 1
path = self.planner.getPath(milestone1, milestone2)
cost = self.planner.pathCost(path)
self.adjacency_matrix[i, j] = cost
self.adjacency_matrix[j, i] = cost
pathDict[i, j] = pathDict[j, i] = path
# print((i,j),(j,i))
print('calculated all distances')
for i in range(self.adjacency_matrix.shape[0]):
self.adjacency_matrix[i, i] = 0
return self.adjacency_matrix, pathDict
class CSpaceObstacleProgram(GLProgram):
def __init__(self,
space,
start=(0.1, 0.5),
goal=(0.9, 0.5),
x_bound=1.0,
y_bound=1.0,
milestones=(0, 0),
initial_points=1000):
GLProgram.__init__(self)
self.space = space
#PRM planner
MotionPlan.setOptions(type="prm",
knn=10,
connectionThreshold=1,
ignoreConnectedComponents=True)
self.optimizingPlanner = False
# we first plan a bit without goals just to have a good skeleton
self.initial_points = initial_points
self.x_bound = x_bound
self.y_bound = y_bound
self.milestones = milestones
self.times = 0
self.planner = MotionPlan(space)
print('planning initial roadmap with {} points'.format(initial_points))
self.planner.planMore(self.initial_points)
self.connected = False
#FMM* planner
#MotionPlan.setOptions(type="fmm*")
#self.optimizingPlanner = True
#RRT planner
#MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True)
#self.optimizingPlanner = False
# RRT* planner
# MotionPlan.setOptions(type="rrt*")
# self.optimizingPlanner = True
#random-restart RRT planner
#MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True,shortcut=True,restart=True,restartTermCond="{foundSolution:1,maxIters:1000}")
#self.optimizingPlanner = True
#OMPL planners:
#Tested to work fine with OMPL's prm, lazyprm, prm*, lazyprm*, rrt, rrt*, rrtconnect, lazyrrt, lbtrrt, sbl, bitstar.
#Note that lbtrrt doesn't seem to continue after first iteration.
#Note that stride, pdst, and fmt do not work properly...
#MotionPlan.setOptions(type="ompl:rrt",suboptimalityFactor=0.1,knn=10,connectionThreshold=0.1)
#self.optimizingPlanner = True
# we then add start, goal and milestones
self.start = start
self.goal = goal
# self.planner.setEndpoints(start,goal)
# we now add each of the chosen points as a milestone:
self.G = self.planner.getRoadmap()
self.start_milestones = len(self.G[0])
print(self.start_milestones)
for milestone in self.milestones:
self.planner.addMilestone(milestone)
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
self.G = self.planner.getRoadmap()
self.end_milestones = len(self.G[0])
print(self.end_milestones)
# self.planner.addMilestone(self.start)
# self.planner.addMilestone(self.goal)
self.path = []
self.G = None
def keyboardfunc(self, key, x, y):
if key == ' ':
if ((self.optimizingPlanner or not self.path)
or (self.components > 1)):
print("Planning 1...")
self.planner.planMore(1)
self.path = self.planner.getPath()
self.G = self.planner.getRoadmap()
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
self.refresh()
elif key == 'p':
if ((self.optimizingPlanner or not self.path)
or (self.components > 1)):
print("Planning 100...")
self.planner.planMore(1000)
self.path = self.planner.getPath()
self.G = self.planner.getRoadmap()
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
self.paths = []
# for i in range(self.start_milestones,self.end_milestones):
# for j in range(i,self.end_milestones):
# print('getting paths')
# self.paths.append( self.planner.getPath(i,j))
self.refresh()
# elif key=='g':
# adjacency_matrix = np.zeros(shape = (len(self.milestones),len(self.milestones)))
# adjacency_matrix[:,:] = np.inf
# if(self.components == 1):
# for i,milestone1 in tqdm(enumerate(range(self.start_milestones+1,len(self.milestones)))):
# for j,milestone2 in enumerate(range(milestone1+1,len(self.milestones))):
# path = self.planner.getPath(milestone1,milestone2)
# cost = self.planner.pathCost(path)
# adjacency_matrix[i,j] = cost
# adjacency_matrix[j,i] = cost
# print('calculated all distances')
# return adjacency_matrix
def display(self):
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0, self.x_bound, self.y_bound, 0, -1, 1)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glDisable(GL_LIGHTING)
self.space.drawObstaclesGL()
if ((self.path) and (self.components == 1)):
self.paths = []
for i in range(self.start_milestones, self.end_milestones):
for j in range(i + 1, self.end_milestones):
print('getting paths')
self.paths.append(self.planner.getPath(i, j))
#draw path
# glColor3f(0,1,0)
# glBegin(GL_LINE_STRIP)
self.colors = []
for i, path in enumerate(self.paths):
if (len(self.colors) < i + 1):
self.colors.append(
[np.random.rand(),
np.random.rand(),
np.random.rand()])
glColor3f(*self.colors[i])
glBegin(GL_LINE_STRIP)
for q in path:
glVertex2f(q[0], q[1])
glEnd()
# for path in self.paths:
# for q in path:
# self.space.drawRobotGL(q)
for milestone in self.milestones:
self.space.drawRobotGL(milestone)
else:
for milestone in self.milestones:
self.space.drawRobotGL(milestone)
pass
if self.G:
#draw graph
V, E = self.G
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(0, 0, 0, 0.5)
glPointSize(3.0)
glBegin(GL_POINTS)
for v in V:
glVertex2f(v[0], v[1])
glEnd()
glColor4f(0.5, 0.5, 0.5, 0.5)
glBegin(GL_LINES)
for (i, j) in E:
glVertex2f(V[i][0], V[i][1])
glVertex2f(V[j][0], V[j][1])
glEnd()
glDisable(GL_BLEND)
class CSpaceObstacleSolver1:
def __init__(self,
space,
milestones=(0, 0),
initial_points=4000,
steps=100):
self.space = space
#PRM planner
MotionPlan.setOptions(type="prm",
knn=10,
connectionThreshold=1,
ignoreConnectedComponents=True)
self.optimizingPlanner = False
# we first plan a bit without goals just to have a good skeleton
self.initial_points = initial_points
self.steps = steps
self.milestones = milestones
self.times = 0
self.planner = MotionPlan(space)
for milestone in self.milestones:
self.planner.addMilestone(milestone)
self.components = int(self.planner.getStats()['numComponents'])
print(self.components)
print('planning initial roadmap with {} points'.format(initial_points))
self.planner.planMore(self.initial_points)
self.connected = False
# we now add each of the chosen points as a milestone:
self.G = self.planner.getRoadmap()
self.start_milestones = len(self.G[0])
self.path = []
self.milestone_2 = 1
self.G = None
self.count = 0
self.connected_list = {0}
self.total_milestones = set(list(range(len(self.milestones))))
def compute_actual_distance(self, origins, ends):
# weights = []
weights = np.linalg.norm(ends - origins, axis=1)
return weights
def compute_real_pairwise_distances(self, G_list):
G = nx.Graph()
G.add_nodes_from(range(len(G_list[0])))
G.add_edges_from(G_list[1])
edges = np.array(G_list[1])
nodes = np.array(G_list[0])
# for config,node in zip(G_list[0],nodes):
# G.nodes[node]['config'] = config
origins = nodes[edges[:, 0]]
ends = nodes[edges[:, 1]]
weights = self.compute_actual_distance(origins, ends)
for weight, edge in zip(weights, edges):
G.edges[edge]['weight'] = weight
print('actually calculating distances')
self.G = G
distances_array = []
for i in tqdm(range(len(self.milestones))):
distances_dict = dict(
nx.algorithms.shortest_path_length(G,
source=i,
weight='weight'))
this_distance = []
for j in range(len(self.milestones)):
this_distance.append(distances_dict[j])
distances_array.append(this_distance)
distances = np.array(distances_array)
# self.G = G
return distances
def get_adjacency_matrix_from_milestones(self):
while (self.connected == False):
if (self.space.fraction > 0.2):
print("Planning {}... components = {}".format(
self.steps, int(self.planner.getStats()['numComponents'])))
else:
print("Focused Planning {}... components = {}".format(
self.steps, int(self.planner.getStats()['numComponents'])))
self.planner.planMore(self.steps)
milestone_1 = 0
remaining = self.total_milestones - self.connected_list
G_list = self.planner.getRoadmap()
G = nx.Graph()
# self.space.set_remaining_milestones(remaining,G_list)
G.add_nodes_from(range(len(G_list[0])))
G.add_edges_from(G_list[1])
elements_with_zero = nx.algorithms.node_connected_component(G, 0)
self.connected_list = self.total_milestones.intersection(
elements_with_zero)
print('connected so far: ', len(self.connected_list))
# self.components =
# for milestone in remaining:
# # print(milestone_1,milestone)
# if(not(self.planner.getPath(milestone_1,milestone) == None)):
# self.connected_list.update([milestone])
if (self.connected_list == self.total_milestones):
self.connected = True
if (self.space.fraction < 0.2):
self.steps = 20
print(
'PRM connecting all milestones found - computing adjacency matrix')
pathDict = dict()
self.adjacency_matrix = np.zeros(shape=(len(self.milestones),
len(self.milestones)))
# self.adjacency_matrix[:,:] = 0
# for i,milestone1 in tqdm(enumerate(range(0,1 + len(self.milestones)))):
# # print(self.G[0][milestone1])
# for j,milestone2 in enumerate(range(milestone1+1,len(self.milestones))):
# j = j+i + 1
# path = self.planner.getPath(milestone1,milestone2)
# cost = self.planner.pathCost(path)
# self.adjacency_matrix[i,j] = cost
# self.adjacency_matrix[j,i] = cost
# pathDict[i,j] = pathDict[j,i] = path
# print((i,j),(j,i))
rm = self.planner.getRoadmap()
self.adjacency_matrix = self.compute_real_pairwise_distances(rm)
print('calculated all distances')
return self.adjacency_matrix, rm
class CSpaceObstacleSolver:
def __init__(self,
space,
milestones=(0, 0),
initial_points=4000,
steps=100):
self.space = space
#PRM planner
MotionPlan.setOptions(type="prm",
knn=20,
connectionThreshold=0.5,
ignoreConnectedComponents=True)
self.optimizingPlanner = False
# we first plan a bit without goals just to have a good skeleton
self.initial_points = initial_points
self.steps = steps
self.milestones = milestones
self.times = 0
self.planner = MotionPlan(space)
for milestone in self.milestones:
self.planner.addMilestone(milestone.tolist())
self.components = int(self.planner.getStats()['numComponents'])
# print(self.components)
print('planning initial roadmap with {} points'.format(initial_points))
self.planner.planMore(self.initial_points)
self.connected = False
# we now add each of the chosen points as a milestone:
self.G = self.planner.getRoadmap()
self.start_milestones = len(self.G[0])
self.path = []
self.milestone_2 = 1
self.G = None
self.count = 0
self.connected_list = {0}
self.total_milestones = set(list(range(len(self.milestones))))
def compute_actual_distance(self, origins, ends):
weights = []
for origin, end in zip(origins, ends):
interp = self.space.interp(origin, end)
interp = interp[:, :self.space.robot.numLinks()]
positions = []
for cfig in interp:
self.space.robot.setConfig(cfig)
positions.append(self.space.lamp.getTransform()[1])
positions = np.array(positions)
distance = np.linalg.norm(np.diff(positions, axis=0), axis=1).sum()
weights.append(distance)
return weights
def compute_real_pairwise_distances(self, G_list):
G = nx.Graph()
G.add_nodes_from(range(len(G_list[0])))
G.add_edges_from(G_list[1])
edges = np.array(G_list[1])
nodes = np.array(G_list[0])
origins = nodes[edges[:, 0], :self.space.robot.numLinks()]
ends = nodes[edges[:, 1], :self.space.robot.numLinks()]
weights = self.compute_actual_distance(origins, ends)
for weight, edge in zip(weights, edges):
G.edges[edge]['weight'] = weight
distances_array = []
for i in tqdm(range(self.milestones.shape[0])):
distances_dict = dict(
nx.algorithms.shortest_path_length(G,
source=i,
weight='weight'))
this_distance = []
for j in range(self.milestones.shape[0]):
this_distance.append(distances_dict[j])
distances_array.append(this_distance)
distances = np.array(distances_array)
self.G = G
return distances
def get_adjacency_matrix_from_milestones(self):
while (self.connected == False):
if (self.space.fraction > 0.3):
print("Planning {}... components = {}".format(
self.steps, int(self.planner.getStats()['numComponents'])))
else:
print("Focused Planning {}... components = {}".format(
self.steps, int(self.planner.getStats()['numComponents'])))
self.planner.planMore(self.steps)
milestone_1 = 0
remaining = self.total_milestones - self.connected_list
G_list = self.planner.getRoadmap()
if (len(G_list[0]) > 2500):
raise UnreachablePointsError(
'There are points in the planner that are not feasible after 2500 samples!'
)
G = nx.Graph()
self.space.set_remaining_milestones(remaining, G_list)
G.add_nodes_from(range(len(G_list[0])))
G.add_edges_from(G_list[1])
elements_with_zero = nx.algorithms.node_connected_component(G, 0)
self.connected_list = self.total_milestones.intersection(
elements_with_zero)
print('Remaining to connect: {}'.format(
len(self.milestones) - len(self.connected_list)))
if (self.connected_list == self.total_milestones):
self.connected = True
if (self.space.fraction < 0.3):
self.steps = 20
print(
'PRM connecting all milestones found - computing adjacency matrix')
rm = self.planner.getRoadmap()
self.adjacency_matrix = self.compute_real_pairwise_distances(rm)
print('calculated all distances')
return self.adjacency_matrix, self.G, rm[0]