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 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 __init__(self,world,knowledge): self.world = world self.robot = world.robot(0) self.knowledge = knowledge self.collider = WorldCollider(world) #these may be helpful self.left_camera_link = self.robot.getLink(left_camera_link_name) self.right_camera_link = self.robot.getLink(right_camera_link_name) self.left_gripper_link = self.robot.getLink(left_gripper_link_name) self.right_gripper_link = self.robot.getLink(right_gripper_link_name) self.left_arm_links = [self.robot.getLink(i) for i in left_arm_link_names] self.right_arm_links = [self.robot.getLink(i) for i in right_arm_link_names] self.id_to_index = id_to_index = dict([(self.robot.getLink(i).getID(),i) for i in range(self.robot.numLinks())]) self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links] self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links] self.arm_links = { 'right': [ (i, self.robot.getLink(i)) for i in right_arm_geometry_indices + right_hand_geometry_indices ], 'left': [ (i, self.robot.getLink(i)) for i in left_arm_geometry_indices + left_hand_geometry_indices ] } self.robot_links = [ (i, self.robot.getLink(i)) for i in range(self.robot.numLinks()) ] # plan a path using RRT MotionPlan.setOptions(type="rrt",shortcut=1,restart=1) self.cs = {} for limb in [ 'left', 'right' ]: # set up C-space self.cs[limb] = LimbCSpace(self, limb) self.cs[limb].setup()
def getCollisionFreePath(self, start, goal, iterations): """ Given a start and a goal configuration, returns a collision-free path between the two configurations""" """ Currently takes forever to find a path... Needs more work""" #MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True) #MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25) MotionPlan.setOptions(type="sbl", perturbationRadius=2.0, connectionThreshold=0.5, bidirectional=True) #MotionPlan.setOptions(type="lazyrrg*") #space = ObstacleCSpace(self.collider, self.robot) #planner = MotionPlan(space) #planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1) space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world)) jointLimits = self.robot.getJointLimits() lower = jointLimits[0] higher = jointLimits[1] for i in range(12): lower[i] = 0 higher[i] = 0 newLimits = (lower, higher) space.bound = zip(*newLimits) planner = cspace.MotionPlan(space) planner.setEndpoints(start, goal) planner.planMore(iterations) V, E = planner.getRoadmap() self.roadMap = (V, E) return planner.getPath()
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 __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 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 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, 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 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,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
def getCollisionFreePath(self, start, goal, iterations): #MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True) #MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25) MotionPlan.setOptions(type="sbl", perturbationRadius=2.0, connectionThreshold=0.5, bidirectional=True) #MotionPlan.setOptions(type="lazyrrg*") #space = ObstacleCSpace(self.collider, self.robot) #planner = MotionPlan(space) #planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1) print "milestone 1" space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world)) jointLimits = self.robot.getJointLimits() lower = jointLimits[0] higher = jointLimits[1] for i in range(12): lower[i] = 0 higher[i] = 0 newLimits = (lower, higher) space.bound = zip(*newLimits) print "milestone 2" planner = cspace.MotionPlan(space) print "milestone 3" planner.setEndpoints(start, goal) print "before planning" planner.planMore(iterations) print "after planning" V, E = planner.getRoadmap() self.roadMap = (V, E) print "No. of vertices:", len(V) print "Vertices:", V print "Edges:", E return planner.getPath()
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
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