def plan_limb_transfer(self,limb,limbstart,limbgoal,heldobject,grasp):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal while holding the given object.
Returns False if planning failed"""
self.rebuild_dynamic_objects()
cspace = TransferCSpace(self,limb,heldobject,grasp)
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return False
MotionPlan.setOptions(connectionThreshold=5.0)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 200
maxSmoothIters = 10
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
print " Found a path on iteration",iters
if len(path) > 2:
print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
return path
cspace.close()
plan.close()
print "No path found"
return False
def plan_limb(self,limb,limbstart,limbgoal,ignore=None):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
# update the ignored opbjects list
self.cs[limb].ignore = ignore
mp = MotionPlan(self.cs[limb])
try:
mp.setEndpoints(limbstart, limbgoal)
# need this to catch the RuntimeError raised when the goal is infeasible
except RuntimeError as e:
print 'Error:', e
return None
# try 10 rounds of 10 iterations
# if a path isn't found in 50 iterations, report failure
# so a new IK solution is attempted
for i in range(10):
mp.planMore(10)
path = mp.getPath()
# give the simulation a chance to keep up
time.sleep(0.01)
if path:
break
# clean up
mp.close()
if path:
# ensure the path starts with the initial position
if path[0] != limbstart:
path.insert(0, limbstart)
for q in path:
if len(q) < len(limbstart):
print 'fix length'
q.extend([0]*(len(limbstart) - len(q)))
if not self.check_limb_collision_free(limb, q, ignore):
print 'collision (coarse)!'
return None
# the robot can't keep up if the path is too coarsely sampled
# thus, linearly interpolate between "distant" poses in joint space
fine_path = self.resample_path(path, pi/50)
for q in fine_path:
# it seems that the path planner sometimes produces motion plans
# that when finely linearly interpolated have collision states
# thus, check for those collisions and reject the plan
if not self.check_limb_collision_free(limb, q, ignore):
print 'collision (fine)!'
return None
return fine_path
else:
return None
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..."
(res, iters) = solver.solve(100, 1e-3)
if res:
qpregrasp = robot.getConfig()
qpregrasparm = [qpregrasp[i] for i in hand.armIndices]
if not cspace.feasible(qpregrasparm):
print "Pregrasp config infeasible"
return None
if qpregrasp == None:
print "Pregrasp solve failed"
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
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
#add a path to the grasp configuration
return path + [hand.open(path[-1], 0)]
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..."
(res,iters) = solver.solve(100,1e-3);
if res:
qpregrasp = robot.getConfig()
qpregrasparm = [qpregrasp[i] for i in hand.armIndices]
if not cspace.feasible(qpregrasparm):
print "Pregrasp config infeasible"
return None
if qpregrasp == None:
print "Pregrasp solve failed"
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
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
#add a path to the grasp configuration
return path + [hand.open(path[-1],0)]
def plan_limb(self,limb,limbstart,limbgoal, printer=True):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE:
cspace = LimbCSpace(self,limb)
cspaceTest = RobotCSpaceTest(self,limb)
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
MotionPlan.setOptions(connectionThreshold=5.0)
MotionPlan.setOptions(shortcut=1)
# plan = MotionPlan(cspace,'sbl')
# MotionPlan.setOptions(type='rrt*')
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1,shortcut=True)
# MotionPlan.setOptions(type='fmm*')
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 200
maxSmoothIters = 10
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
if printer:
print " Found a path on iteration",iters
if len(path) > 2:
print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
def plan_limb(self,limb,limbstart,limbgoal):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
#TODO: implement me.
#Hint: you should implement the hooks in ArmCSpace above, and consult
#use the MotionPlan class in klampt.cspace to run an existing
#planner.
rrt_planner = MotionPlan(LimbCSpace(self, limb), type="rrt")
rrt_planner.setEndpoints(limbstart, limbgoal)
print "Planning more"
while rrt_planner.getPath() is None:
print "iter...",random.random()
rrt_planner.planMore(10)
print "Planning more done"
print rrt_planner.getPath()
return [limbstart,limbgoal]
class CSpaceObstacleProgram(GLProgram):
def __init__(self,space,start=(0.1,0.5),goal=(0.9,0.5)):
GLProgram.__init__(self)
self.space = space
MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1)
#MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True)
self.planner = MotionPlan(space)
self.start=start
self.goal=goal
self.planner.setEndpoints(start,goal)
self.path = []
def keyboardfunc(self,key,x,y):
if key==' ':
if not self.path:
print "Planning 1..."
self.planner.planMore(1)
self.path = self.planner.getPath()
glutPostRedisplay()
elif key=='p':
if not self.path:
print "Planning 100..."
self.planner.planMore(100)
self.path = self.planner.getPath()
glutPostRedisplay()
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:
glColor3f(0,1,0)
glBegin(GL_LINE_STRIP)
for q in self.path:
print q
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)
def plan_limb(self,limb,limbstart,limbgoal,Cspace='Normal',argsTuple=None,extra_feasibility_tests=[]):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
if(Cspace == 'scoop'):
cspace = ScoopCSpace(self,limb)
elif(Cspace == 'height'):
cspace = HeightCSpace(self,limb,argsTuple[0],argsTuple[1])
elif(Cspace == 'xrestrict'):
cspace = CSpaceRestrictX(self,limb,xrestrict=argsTuple[0])
else:
cspace = LimbCSpace(self,limb)
cspace.extra_feasibility_tests = extra_feasibility_tests
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
visualization.debug(self.world)
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
visualization.debug(self.world)
return False
MotionPlan.setOptions(connectionThreshold=3.5,perturbationRadius=1)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 2000
maxSmoothIters = 50
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
#print " Found a path on iteration",iters
if len(path) >= 2 and maxSmoothIters > 0:
#print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
#print "Average collision check time",cspace.collisionCheckTime/cspace.numCollisionChecks
#print cspace.numCollisionChecks,"collision checks"
return path
#print "Average collision check time",cspace.collisionCheckTime/cspace.numCollisionChecks
#print cspace.numCollisionChecks,"collision checks"
cspace.close()
plan.close()
#print " No path found"
return False
def plan_limb_transfer(self,limb,limbstart,limbgoal,heldobject,grasp,type='Normal',xrestrict=None,extra_feasibility_tests=[]):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal while holding the given object.
Returns False if planning failed"""
if(type == 'Normal'):
cspace = TransferCSpace(self,limb,heldobject,grasp,height=xrestrict)
elif(type == 'restrict'):
cspace = TransferCSpaceRestrict(self,limb,heldobject,grasp)
elif(type == 'upright'):
cspace = TransferCSpaceUpright(self,limb,heldobject,grasp)
elif(type == 'xrestrict'):
cspace = TransferCSpaceRestrict(self,limb,heldobject,grasp,xrestrict)
cspace.extra_feasibility_tests = extra_feasibility_tests
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return False
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return False
MotionPlan.setOptions(connectionThreshold=3.5,perturbationRadius=1)
MotionPlan.setOptions(shortcut=1)
plan = MotionPlan(cspace,'sbl')
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 2000
maxSmoothIters = 50
for iters in xrange(maxPlanIters):
plan.planMore(1)
path = plan.getPath()
if path != None:
#print " Found a path on iteration",iters
if len(path) > 2 and maxSmoothIters > 0:
#print " Smoothing..."
plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
path = plan.getPath()
cspace.close()
plan.close()
return path
cspace.close()
plan.close()
#print "No path found"
return False
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
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 plan_limb(self, limb, limbstart, limbgoal):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
# TODO: implement me.
# Hint: you should implement the hooks in LimbCSpace above, and consult/
# use the MotionPlan class in klampt.cspace to run an existing
# planner.
space = LimbCSpace(self, limb)
MotionPlan.setOptions(type="rrt", connectionThreshold=5, bidirectional=True)
motionPlan = MotionPlan(space)
motionPlan.setEndpoints(limbstart, limbgoal)
t1 = time.clock()
motionPlan.planMore(500)
print time.clock() - t1
path = motionPlan.getPath()
motionPlan.close()
space.close()
if path == None:
return False
else:
print len(path)
return path
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
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
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
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()
glutPostRedisplay()
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()
glutPostRedisplay()
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:
print q
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)
def plan_limb(self,limb,limbstart,limbgoal, printer=True, iks = None):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE: Not sure, but I think this is what's happening
# Need to reset pathToDraw here because the MyGLViewer.draw() is called
# concurrently, it uses an old path (ex. of left arm) while using the
# new limb (ex. right limb) for drawing the trajectory. This throws an
# Index-Out-of-Range exception for drawing the path
self.pathToDraw = []
# NOTE:
cspace = LimbCSpace(self,limb)
if iks != None:
print "Initializing ClosedLoopCSPace"
cspace = ClosedLoopCSpaceTest(self,limb,iks)
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
# maxPlanIters = 20
# maxSmoothIters = 100
maxPlanIters = 10
maxSmoothIters = 100
print " Planning.",
for iters in xrange(maxPlanIters):
# print iters
if limb=='left':
self.limb_indices = self.left_arm_indices
self.activeLimb = 'left'
else:
self.limb_indices = self.right_arm_indices
self.activeLimb = 'right'
self.roadmap = plan.getRoadmap()
V,E =self.roadmap
print ".",
plan.planMore(10) # 100
path = plan.getPath()
if path != None:
if printer:
print "\n Found a path on iteration",iters
if len(path) > 2:
print " Smoothing path"
# plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
plan.planMore(maxSmoothIters)
path = plan.getPath()
cspace.close()
plan.close()
self.pathToDraw = path
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
def plan_limb(self,limb,limbstart,limbgoal, printer=True, iks = None):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE: Not sure, but I think this is what's happening
# Need to reset pathToDraw here because the MyGLViewer.draw() is called
# concurrently, it uses an old path (ex. of left arm) while using the
# new limb (ex. right limb) for drawing the trajectory. This throws an
# Index-Out-of-Range exception for drawing the path
self.pathToDraw = []
# NOTE:
cspace = LimbCSpace(self,limb)
if iks != None:
print "Initializing ClosedLoopCSPace"
cspace = ClosedLoopCSpaceTest(self,limb,iks)
if not cspace.feasible(limbstart):
cspace.feasible(limbstart)
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
# MotionPlan.setOptions(connectionThreshold=5.0)
# MotionPlan.setOptions(shortcut=1)
# plan = MotionPlan(cspace,'sbl')
# MotionPlan.setOptions(type='rrt*')
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.01,shortcut=True)
# MotionPlan.setOptions(type='fmm*')
# MotionPlan.setOptions(bidirectional = 1)
# MotionPlan.setOptions(type="sbl", perturbationRadius = 0.25, connectionThreshold=2.0, bidirectional = True)
# MotionPlan.setOptions(type="rrt",perturbationRadius=0.1,bidirectional=True)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="rrt*", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True)
MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="prm",knn=1,connectionThreshold=0.01)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace, type='sbl')
# works best for non-ClosedLoopRobotCSpace
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 200
maxSmoothIters = 100
print " Planning.",
for iters in xrange(maxPlanIters):
# print iters
if limb=='left':
self.limb_indices = self.left_arm_indices
self.activeLimb = 'left'
else:
self.limb_indices = self.right_arm_indices
self.activeLimb = 'right'
self.roadmap = plan.getRoadmap()
# print plan.getRoadmap()
V,E =self.roadmap
# print "iter:",iters,"--",len(V),"feasible milestones sampled,",len(E),"edges connected"
# print iters,"/V",len(V),"/E",len(E),
print ".",
plan.planMore(10) # 100
path = plan.getPath()
if path != None:
if printer:
print "\n Found a path on iteration",iters
if len(path) > 2:
print " Smoothing path"
# plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
plan.planMore(maxSmoothIters)
path = plan.getPath()
cspace.close()
plan.close()
# testing
# print " Returning path (limb", self.activeLimb,", (",len(self.limb_indices),"/",len(path[0]),"))"
self.pathToDraw = path
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
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
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()
glutPostRedisplay()
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()
glutPostRedisplay()
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:
print q
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)
def plan_limb(self,limb,limbstart,limbgoal, printer=True, iks = None, ignoreColShelfSpatula=False):
"""Returns a 7-DOF milestone path for the given limb to move from the
start to the goal, or False if planning failed"""
self.rebuild_dynamic_objects()
# NOTE: Not sure, but I think this is what's happening
# Need to reset pathToDraw here because the MyGLViewer.draw() is called
# concurrently, it uses an old path (ex. of left arm) while using the
# new limb (ex. right limb) for drawing the trajectory. This throws an
# Index-Out-of-Range exception for drawing the path
self.pathToDraw = []
if limb=='left':
self.limb_indices = self.left_arm_indices
self.activeLimb = 'left'
else:
self.limb_indices = self.right_arm_indices
self.activeLimb = 'right'
# NOTE:
cspace = LimbCSpace(self,limb)
MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
plan = MotionPlan(cspace)
plan.setEndpoints(limbstart,limbgoal)
maxPlanIters = 20
planMoreVal = 10
if iks != None:
print "Initializing ClosedLoopCSPace"
# cspace = ClosedLoopCSpaceTest(self,limb,iks)
collider = WorldCollider(self.world)
cspace = robotcspace.ClosedLoopRobotCSpace(self.robot, iks, collider)
cspace.eps = 1e-2
cspace.setup()
q = limbstart
qcurrent = self.robot.getConfig()
if len(q) < len(qcurrent):
if len(self.limb_indices) != len(q):
return False
else:
for i in range(len(self.limb_indices)):
qcurrent[self.limb_indices[i]] = q[i]
q = qcurrent
limbstart = q
q = limbgoal
qcurrent = self.robot.getConfig()
if len(q) < len(qcurrent):
if len(self.limb_indices) != len(q):
return False
else:
for i in range(len(self.limb_indices)):
qcurrent[self.limb_indices[i]] = q[i]
q = qcurrent
limbgoal = q
# print "limbstart:", limbstart
# print "limbgoal:", limbgoal
limbstart = cspace.solveConstraints(limbstart)
limbgoal = cspace.solveConstraints(limbgoal)
# print "limbstart:", limbstart
# print "limbgoal:", limbgoal
settings = { 'type':"sbl", 'perturbationRadius':0.5, 'bidirectional':1, 'shortcut':1, 'restart':1, 'restartTermCond':"{foundSolution:1,maxIters:1000}" }
self.robot.setConfig(limbstart)
plan = robotplanning.planToConfig(self.world, self.robot, limbgoal, movingSubset='all', **settings)
maxPlanIters = 1
planMoreVal = 500
if not cspace.feasible(limbstart):
print " Start configuration is infeasible!"
return 1
if not cspace.feasible(limbgoal):
print " Goal configuration is infeasible!"
return 2
# MotionPlan.setOptions(connectionThreshold=5.0)
# MotionPlan.setOptions(shortcut=1)
# plan = MotionPlan(cspace,'sbl')
# MotionPlan.setOptions(type='rrt*')
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.01,shortcut=True)
# MotionPlan.setOptions(type='fmm*')
# MotionPlan.setOptions(bidirectional = 1)
# MotionPlan.setOptions(type="sbl", perturbationRadius = 0.25, connectionThreshold=2.0, bidirectional = True)
# MotionPlan.setOptions(type="rrt",perturbationRadius=0.1,bidirectional=True)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="rrt*", perturbationRadius = 0.25, connectionThreshold=2, bidirectional = True)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# MotionPlan.setOptions(type="prm",knn=1,connectionThreshold=0.01)
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace, type='sbl')
# works best for non-ClosedLoopRobotCSpace
# MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True)
# plan = MotionPlan(cspace)
# maxPlanIters = 20
maxSmoothIters = 100
print " Planning.",
for iters in xrange(maxPlanIters):
# print iters
self.roadmap = plan.getRoadmap()
# print plan.getRoadmap()
V,E =self.roadmap
# print "iter:",iters,"--",len(V),"feasible milestones sampled,",len(E),"edges connected"
# print iters,"/V",len(V),"/E",len(E),
print ".",
plan.planMore(planMoreVal) # 100
path = plan.getPath()
if path != None:
if printer:
print "\n Found a path on iteration",iters
if len(path) > 2:
print " Smoothing path"
if iks == None:
# plan.planMore(min(maxPlanIters-iters,maxSmoothIters))
plan.planMore(maxSmoothIters)
path = plan.getPath()
if iks!=None:
path = cspace.discretizePath(path)
cspace.close()
plan.close()
# testing
# print " Returning path (limb", self.activeLimb,", (",len(self.limb_indices),"/",len(path[0]),"))"
self.pathToDraw = path
return path
cspace.close()
plan.close()
if printer:
print " No path found"
return False
