def plan_closedLoop(self,qstart,qgoal,iks,printer=False):
if printer:
print "starting config: ", qstart
print "goal config: ", qgoal
self.world.terrain(0).geometry().setCollisionMargin(0.05)
cspace = robotcspace.ClosedLoopRobotCSpace(self.robot, iks, self.collider)
cspace.eps = 1e-3
cspace.setup()
configs=[]
configs.append(qstart)
configs.append(qgoal)
configs[0] = cspace.solveConstraints(configs[0])
configs[1] = cspace.solveConstraints(configs[1])
settings = { 'type':"sbl", 'perturbationRadius':0.5, 'bidirectional':1, 'shortcut':1, 'restart':1, 'restartTermCond':"{foundSolution:1,maxIters:1000}" }
wholepath = [configs[0]]
for i in range(len(configs)-1):
#this code uses the robotplanning module's convenience functions
self.robot.setConfig(configs[i])
plan = robotplanning.planToConfig(self.world,self.robot,configs[i+1],
movingSubset='all',
**settings)
if plan is None:
return False
print " Planning..."
keepPlanning = True
while keepPlanning:
plan.planMore(500)
#this code just gives some debugging information. it may get expensive
# V,E = plan.getRoadmap()
# self.roadmap = plan.getRoadmap()
# print " ", len(V),"feasible milestones sampled,",len(E),"edges connected"
path = plan.getPath()
if path is None or len(path)==0:
print "Failed to plan path"
#debug some sampled configurations
# print V[0:max(10,len(V))]
# return False
else:
keepPlanning = False
self.pathToDraw = path
#the path is currently a set of milestones: discretize it so that it stays near the contact surface
path = cspace.discretizePath(path,epsilon=1e-2)
# path = cspace.discretizePath(path,epsilon=1e-4)
wholepath += path[1:]
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return wholepath
def myPlanner(q0, q, settings):
qSim0 = getSimJoints(MOVING_LIMB, q0)
qSim = getSimJoints(MOVING_LIMB, q)
t0 = time.time()
print "Creating plan..."
#this code uses the robotplanning module's convenience functions
ROBOT.setConfig(qSim0)
plan = robotplanning.planToConfig(WORLD,
ROBOT,
qSim,
movingSubset='all',
**settings)
if plan is None:
print 'plan is None...'
return None
print "Planner creation time", time.time() - t0
t0 = time.time()
# motionplanning.CSpaceInterface.enableAdaptiveQueries()
print "Planning..."
for round in range(20):
plan.planMore(50)
print "Planning time, 500 iterations", time.time() - t0
#this code just gives some debugging information. it may get expensive
#V,E = plan.getRoadmap()
#print len(V),"feasible milestones sampled,",len(E),"edges connected"
path = plan.getPath()
if path is None or len(path) == 0:
print "Failed to plan path between configuration"
print q0
print "and"
print q
# #debug some sampled configurations
# print V[0:min(10,len(V))]
"""
print "Constraint testing order:"
print plan.space.cspace.feasibilityQueryOrder()
print "Manually optimizing constraint testing order..."
plan.space.cspace.optimizeQueryOrder()
print "Optimized constraint testing order:"
print plan.space.cspace.feasibilityQueryOrder()
print "Plan stats:"
print plan.getStats()
print "CSpace stats:"
print plan.space.getStats()
"""
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return path
#Generate a path connecting the edited configurations
#You might edit the value 500 to play with how many iterations to give the
#planner.
wholepath = [configs[0]]
for i in range(len(configs) - 1):
if MANUAL_PLAN_CREATION:
#Manual construction of planner
plan = cspace.MotionPlan(space, **settings)
plan.setEndpoints(configs[0], configs[1])
else:
#this code uses the robotplanning module's convenience functions
robot.setConfig(configs[i])
plan = robotplanning.planToConfig(world,
robot,
configs[i + 1],
movingSubset='all',
**settings)
if plan is None:
break
print "Planning..."
plan.planMore(500)
#this code just gives some debugging information. it may get expensive
V, E = plan.getRoadmap()
print len(V), "feasible milestones sampled,", len(E), "edges connected"
path = plan.getPath()
if path is None or len(path) == 0:
print "Failed to plan path between configuration", i, "and", i + 1
#debug some sampled configurations
print V[0:max(10, len(V))]
break
def pathPlanner(self, q0, q):
qSim0 = self.getSimJoints(q0)
qSim = self.getSimJoints(q)
t0 = time.time()
print "Creating plan..."
#this code uses the robotplanning module's convenience functions
settings = self.getPlanSettings()
self.robot.setConfig(qSim0)
plan = robotplanning.planToConfig(self.world,self.robot,qSim,
movingSubset='all',
**settings)
if plan is None:
print 'plan is None...'
return None
print "Planner creation time",time.time()-t0
# motionplanning.CSpaceInterface.enableAdaptiveQueries()
print "Planning..."
if self.planTimes == 0:
self.firstPathPlanner(plan)
else:
t0 = time.time()
for round in range(self.planRounds):
plan.planMore(self.planTimes)
print "Planning time,", self.planRounds*self.planTimes, "iterations",time.time()-t0
self.planningTime = self.planningTime + time.time()-t0
#this code just gives some debugging information. it may get expensive
#V,E = plan.getRoadmap()
#print len(V),"feasible milestones sampled,",len(E),"edges connected"
path = plan.getPath()
if path is None or len(path)==0:
print "Failed to plan path between configuration"
print q0
print "and"
print q
# #debug some sampled configurations
# print V[0:min(10,len(V))]
"""
print "Constraint testing order:"
print plan.space.cspace.feasibilityQueryOrder()
print "Manually optimizing constraint testing order..."
plan.space.cspace.optimizeQueryOrder()
print "Optimized constraint testing order:"
print plan.space.cspace.feasibilityQueryOrder()
print "Plan stats:"
print plan.getStats()
print "CSpace stats:"
print plan.space.getStats()
"""
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
grip = {JSON_GRIP_LEFT: [0,0], JSON_GRIP_RIGHT: [0,0]}
if self.movingLimb == JSON_LEFT:
grip[JSON_GRIP_LEFT][0] = q0[JSON_GRIP_LEFT]
grip[JSON_GRIP_LEFT][1] = q[JSON_GRIP_LEFT]
if self.movingLimb == JSON_RIGHT:
grip[JSON_GRIP_RIGHT][0] = q0[JSON_GRIP_RIGHT]
grip[JSON_GRIP_RIGHT][1] = q[JSON_GRIP_RIGHT]
if self.movingLimb == JSON_BOTH:
grip[JSON_GRIP_RIGHT][0] = q0[JSON_GRIP_RIGHT]
grip[JSON_GRIP_RIGHT][1] = q[JSON_GRIP_RIGHT]
grip[JSON_GRIP_LEFT][0] = q0[JSON_GRIP_LEFT]
grip[JSON_GRIP_LEFT][1] = q[JSON_GRIP_LEFT]
return self.getCommandPath(path, grip)
#Generate a path connecting the edited configurations
#You might edit the value 500 to play with how many iterations to give the
#planner.
wholepath = [configs[0]]
for i in range(len(configs)-1):
if MANUAL_PLAN_CREATION:
#Manual construction of planner
plan = cspace.MotionPlan(space, **settings)
plan.setEndpoints(configs[0],configs[1])
else:
#this code uses the robotplanning module's convenience functions
robot.setConfig(configs[i])
plan = robotplanning.planToConfig(world,robot,configs[i+1],
movingSubset='all',
**settings)
if plan is None:
break
print "Planning..."
plan.planMore(500)
#this code just gives some debugging information. it may get expensive
V,E = plan.getRoadmap()
print len(V),"feasible milestones sampled,",len(E),"edges connected"
path = plan.getPath()
if path is None or len(path)==0:
print "Failed to plan path between configuration",i,"and",i+1
#debug some sampled configurations
print V[0:max(10,len(V))]
break
if CLOSED_LOOP_TEST:
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
