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