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 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(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_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 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 planTransfer(world, objectIndex, hand, shift): """Plan a transfer path for the robot given in world, which is currently holding the object indexed by objectIndex in the hand hand. The desired motion should translate the object by shift without rotating the object. """ globals = Globals(world) obj = world.rigidObject(objectIndex) cspace = TransferCSpace(globals, hand, obj) 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" print "TODO: Complete 2.a to bypass this error" raw_input() return None #TODO: get the ungrasp config using an IK solver qungrasp = None qungrasparm = None print "TODO: Complete 2.b to find a feasible ungrasp config" raw_input() solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift), (0, 0, 1)) #plan the transfer path between q0arm and qungrasparm print "Planning transfer motion to ungrasp config..." MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5) planner = MotionPlan(cspace, 'sbl') planner.setEndpoints(q0arm, qungrasparm) #TODO: do the planning print "TODO: Complete 2.c to find a feasible transfer path" raw_input() #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 qpostungrasp = hand.open(qungrasp, 1.0) return path + [qpostungrasp]
def planTransfer(world,objectIndex,hand,shift): """Plan a transfer path for the robot given in world, which is currently holding the object indexed by objectIndex in the hand hand. The desired motion should translate the object by shift without rotating the object. """ globals = Globals(world) obj = world.rigidObject(objectIndex) cspace = TransferCSpace(globals,hand,obj) 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" print "TODO: Complete 2.a to bypass this error" raw_input() return None #TODO: get the ungrasp config using an IK solver qungrasp = None qungrasparm = None print "TODO: Complete 2.b to find a feasible ungrasp config" raw_input() solver = hand.ikSolver(robot,vectorops.add(obj.getTransform()[1],shift),(0,0,1)) #plan the transfer path between q0arm and qungrasparm print "Planning transfer motion to ungrasp config..." MotionPlan.setOptions(connectionThreshold=5.0,perturbationRadius=0.5) planner = MotionPlan(cspace,'sbl') planner.setEndpoints(q0arm,qungrasparm) #TODO: do the planning print "TODO: Complete 2.c to find a feasible transfer path" raw_input() #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 qpostungrasp = hand.open(qungrasp,1.0) return path + [qpostungrasp]
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
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