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