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()
def planTransfer(world, objectIndex, hand, shift):
"""Plan a transfer path for the robot given in world, which is currently
holding the object indexed by objectIndex in the hand hand.
The desired motion should translate the object by shift without rotating
the object.
"""
globals = Globals(world)
obj = world.rigidObject(objectIndex)
cspace = TransferCSpace(globals, hand, obj)
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"
print "TODO: Complete 2.a to bypass this error"
raw_input()
cspace.close()
return None
#TODO: get the ungrasp config using an IK solver
qungrasp = None
qungrasparm = None
print "TODO: Complete 2.b to find a feasible ungrasp config"
raw_input()
solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift),
(0, 0, 1))
#plan the transfer path between q0arm and qungrasparm
print "Planning transfer motion to ungrasp config..."
MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5)
planner = MotionPlan(cspace, 'sbl')
planner.setEndpoints(q0arm, qungrasparm)
#TODO: do the planning
print "TODO: Complete 2.c to find a feasible transfer path"
raw_input()
cspace.close()
#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
qpostungrasp = hand.open(qungrasp, 1.0)
return path + [qpostungrasp]
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)):
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)