def planTransit(world, objectIndex, hand):
globals = Globals(world)
cspace = TransitCSpace(globals, hand)
obj = world.rigidObject(objectIndex)
robot = world.robot(0)
qmin, qmax = robot.getJointLimits()
#get the start config
q0 = robot.getConfig()
q0arm = [q0[i] for i in hand.armIndices]
if not cspace.feasible(q0arm):
print "Warning, arm start configuration is infeasible"
#get the pregrasp config -- TODO: what if the ik solver doesn't work?
qpregrasp = None
qpregrasparm = None
solver = hand.ikSolver(robot, obj.getTransform()[1], [0, 0, 1])
print "Trying to find pregrasp config..."
solver.setMaxIters(100)
solver.setTolerance(1e-3)
res = solver.solve()
if res:
qpregrasp = robot.getConfig()
qpregrasparm = [qpregrasp[i] for i in hand.armIndices]
if not cspace.feasible(qpregrasparm):
print "Pregrasp config infeasible"
cspace.close()
return None
if qpregrasp == None:
print "Pregrasp solve failed"
cspace.close()
return None
print "Planning transit motion to pregrasp config..."
MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5)
planner = MotionPlan(cspace, 'sbl')
planner.setEndpoints(q0arm, qpregrasparm)
iters = 0
step = 10
while planner.getPath() == None and iters < 1000:
planner.planMore(step)
iters += step
cspace.close()
if planner.getPath() == None:
print "Failed finding transit path"
return None
print "Success, found path with", len(planner.getPath()), "milestones"
#lift arm path to whole configuration space path
path = []
for qarm in planner.getPath():
path.append(q0[:])
for qi, i in zip(qarm, hand.armIndices):
path[-1][i] = qi
#add a path to the grasp configuration
return path + [hand.open(path[-1], 0)]
def planFree(world, hand, qtarget):
"""Plans a free-space motion for the robot's arm from the current
configuration to the destination qtarget"""
globals = Globals(world)
cspace = TransitCSpace(globals, hand)
robot = world.robot(0)
qmin, qmax = robot.getJointLimits()
#get the start/goal config
q0 = robot.getConfig()
q0arm = [q0[i] for i in hand.armIndices]
qtargetarm = [qtarget[i] for i in hand.armIndices]
if not cspace.feasible(q0arm):
print "Warning, arm start configuration is infeasible"
if not cspace.feasible(qtargetarm):
print "Warning, arm goal configuration is infeasible"
print "Planning transit motion to target config..."
MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5)
planner = MotionPlan(cspace, 'sbl')
planner.setEndpoints(q0arm, qtargetarm)
iters = 0
step = 10
while planner.getPath() == None and iters < 1000:
planner.planMore(step)
iters += step
cspace.close()
if planner.getPath() == None:
print "Failed finding transit path"
return None
print "Success"
#lift arm path to whole configuration space path
path = []
for qarm in planner.getPath():
path.append(q0[:])
for qi, i in zip(qarm, hand.armIndices):
path[-1][i] = qi
return path
def testPlannerSuccessRate(N=100,
duration=10,
spawnFunc=lambda: kinkTest(0.0025, False)):
import time
import matplotlib.pyplot as plt
finished = []
for run in range(N):
space, s, g = spawnFunc()
space.eps = 1e-3
if run == 0 and False: #show space
space.drawObstaclesMPL(plt.axes())
plt.scatter([s[0], g[0]], [s[1], g[1]])
plt.show()
plan = MotionPlan(space, type='prm', knn=5)
plan.setEndpoints(s, g)
t0 = time.time()
finished.append(None)
while time.time() - t0 < duration:
plan.planMore(5)
if plan.getPath():
finished[-1] = time.time() - t0
print("Found path with", len(plan.getPath()), "milestones in",
time.time() - t0, "s")
break
if finished[-1] is None:
print("Failed to find path in", duration, "s")
import numpy as np
finished = [v for v in finished if v != None]
hist, edges = np.histogram(finished, 20, (0, duration))
print(hist, edges)
hist = hist * 100 / N
chist = np.cumsum(hist)
plt.bar(edges[:-1], 100 - chist, duration / 20)
plt.xlabel('Time (s)')
plt.ylabel('% failed')
plt.xlim(0, duration)
plt.ylim(0, 100)
plt.savefig('histogram.png')
plt.show()
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 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 CSpaceObstacleProgram(GLProgram):
def __init__(self, space, start=(0.1, 0.5), goal=(0.9, 0.5)):
GLProgram.__init__(self)
self.space = space
#PRM planner
MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.1)
self.optimizingPlanner = 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
self.planner = MotionPlan(space)
self.start = start
self.goal = goal
self.planner.setEndpoints(start, goal)
self.path = []
self.G = None
def keyboardfunc(self, key, x, y):
if key == ' ':
if self.optimizingPlanner or not self.path:
print "Planning 1..."
self.planner.planMore(1)
self.path = self.planner.getPath()
self.G = self.planner.getRoadmap()
self.refresh()
elif key == 'p':
if self.optimizingPlanner or not self.path:
print "Planning 100..."
self.planner.planMore(100)
self.path = self.planner.getPath()
self.G = self.planner.getRoadmap()
self.refresh()
def display(self):
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0, 1, 1, 0, -1, 1)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glDisable(GL_LIGHTING)
self.space.drawObstaclesGL()
if self.path:
#draw path
glColor3f(0, 1, 0)
glBegin(GL_LINE_STRIP)
for q in self.path:
glVertex2f(q[0], q[1])
glEnd()
for q in self.path:
self.space.drawRobotGL(q)
else:
self.space.drawRobotGL(self.start)
self.space.drawRobotGL(self.goal)
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 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 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]
