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 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,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 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.
# Create limb cspace and new motion plan
limb_cspace = LimbCSpace(self, limb)
motion_plan = MotionPlan(limb_cspace, 'prm')
# Set start and endpoints and get path between them
motion_plan.setEndpoints(limbstart, limbgoal)
path = motion_plan.getPath()
# Memory cleanup
limb_cspace.close()
motion_plan.close()
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 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, 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
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
