class LimbPlanner: """Much of your code for HW4 will go here. Attributes: - world: the WorldModel - robot: the RobotModel - knowledge: the KnowledgeBase objcet - collider: a WorldCollider object (see the klampt.robotcollide module) """ 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.link(left_camera_link_name) self.right_camera_link = self.robot.link(right_camera_link_name) self.left_gripper_link = self.robot.link(left_gripper_link_name) self.right_gripper_link = self.robot.link(right_gripper_link_name) self.left_arm_links = [self.robot.link(i) for i in left_arm_link_names] self.right_arm_links = [self.robot.link(i) for i in right_arm_link_names] id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())]) self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices self.right_arm_indices = right_arm_geometry_indices + right_hand_geometry_indices self.dynamic_objects = [] self.roadmap = ([],[]) self.limb_indices = [] self.pathToDraw = [] self.colMargin = 0 self.activeLimb = None def set_limb_config(self,limb,limbconfig,q): """Helper: Sets the 7-DOF configuration of the given limb in q. Other DOFs are not touched.""" assert len(q) == self.robot.numLinks() if limb=='left': for (i,v) in zip(self.left_arm_indices,limbconfig): q[i] = v else: for (i,v) in zip(self.right_arm_indices,limbconfig): q[i] = v def get_limb_config(self,q,limb): """Helper: Gets the 7-DOF configuration of the given limb given a full-robot configuration q""" qlimb = [0.0]*len(self.left_arm_indices) if limb=='left': qlimb = [0.0]*len(self.left_arm_indices) for (i,j) in enumerate(self.left_arm_indices): qlimb[i] = q[j] else: qlimb = [0.0]*len(self.right_arm_indices) for (i,j) in enumerate(self.right_arm_indices): qlimb[i] = q[j] return qlimb def check_collision_free(self,limb): """Checks whether the given limb is collision free at the robot's current configuration""" armfilter = None if limb=='left': collindices = set(left_arm_geometry_indices+left_hand_geometry_indices) # collindices = set(self.left_arm_indices) else: collindices = set(right_arm_geometry_indices+right_hand_geometry_indices) #print "collindices", collindices armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) ignoreList = ["Amazon_Picking_Shelf", "bin_"] ignoreList = '\t'.join(ignoreList) spatulaIgnoreList = [57] #check with objects in world model for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): # NOTE: what is bb_reject?? #print "Collision Test: Collision between",o1[0].getName(),o2[0].getName() #print "Collision Test: Collision for ",o2[0].getName() #print o1[0].index, o2[0].index if o1[0].getName()[0:3] in ignoreList and o2[0].index in spatulaIgnoreList: # print "ignoring collision between shelf and spautla" continue if o2[0].getName()[0:3] in ignoreList and o1[0].index in spatulaIgnoreList: # print "ignoring collision between shelf and spautla" continue if o1[0].getName()[0:3] in ignoreList: for obj in self.dynamic_objects: assert obj.info.geometry != None if o1[1].collides(obj.info.geometry): continue if o2[0].getName()[0:3] in ignoreList: for obj in self.dynamic_objects: assert obj.info.geometry != None if o1[1].collides(obj.info.geometry): continue if (o1[0].index == 15 and o2[0].index == 33) or (o2[0].index == 15 and o1[0].index == 33): continue if (o1[0].index == 35 and o2[0].index == 33) or (o2[0].index == 35 and o1[0].index == 33): continue if o1[1].collides(o2[1]): # print "Collision between",o1[0].getName(),o2[0].getName() # print "Collision between",o1[0].index,o2[0].index return False return True def rebuild_dynamic_objects(self): # self.dynamic_objects = [] # #check with objects in knowledge # for (k,objList) in self.knowledge.bin_contents.iteritems(): # if objList == None: # #not sensed # continue # for item in objList: # assert item.info.geometry != None # item.info.geometry.setCurrentTransform(*item.xform) # self.dynamic_objects.append(item) return def check_limb_collision_free(self,limb,limbconfig): """Checks whether the given 7-DOF limb configuration is collision free, keeping the rest of self.robot fixed.""" q = self.robot.getConfig() self.set_limb_config(limb,limbconfig,q) self.robot.setConfig(q) return self.check_collision_free(limb) 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(self,start,goal,limb,printer=True, iks = None, ignoreColShelfSpatula = True): """Plans a motion for the robot to move from configuration start to configuration goal. By default, moves the left arm first, then the right. To move the right first, set the 'order' argument to ['right','left']""" limbstart = {} limbgoal = {} # for l in ['left','right']: l =limb limbstart[l] = self.get_limb_config(start,l) limbgoal[l] = self.get_limb_config(goal,l) path = [start] curconfig = start[:] diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l])) if diff > 1e-8: if printer: print "< Planning for limb",l,">" # print " Euclidean distance:",math.sqrt(diff) self.robot.setConfig(curconfig) #do the limb planning if not ignoreColShelfSpatula: self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices + [55,56,57] else: self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices # print " Left arm links", self.left_arm_indices if iks == None: limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks) else: trajectory = self.plan_closedLoop(start,goal,iks=iks) return trajectory if limbpath == 1 or limbpath == 2 or limbpath == False: if printer: print " Failed to plan for limb",l,"\n" return limbpath if printer: print " Planned successfully for limb",l, "\n" #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(l,qlimb,q) path.append(q) self.set_limb_config(l,limbgoal[l],curconfig) return path 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
class LimbPlanner: """Much of your code for HW4 will go here. Attributes: - world: the WorldModel - robot: the RobotModel - knowledge: the KnowledgeBase objcet - collider: a WorldCollider object (see the klampt.robotcollide module) """ 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(baxter.left_camera_link_name) self.right_camera_link = self.robot.getLink(baxter.right_camera_link_name) self.left_gripper_link = self.robot.getLink(baxter.left_gripper_link_name) self.right_gripper_link = self.robot.getLink(baxter.right_gripper_link_name) self.left_arm_links = [self.robot.getLink(i) for i in baxter.left_arm_link_names] self.right_arm_links = [self.robot.getLink(i) for i in baxter.right_arm_link_names] 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.dynamic_objects = [] def set_limb_config(self,limb,limbconfig,q): """Helper: Sets the 7-DOF configuration of the given limb in q. Other DOFs are not touched.""" assert len(q) == self.robot.numLinks() if limb=='left': for (i,v) in zip(self.left_arm_indices,limbconfig): q[i] = v else: for (i,v) in zip(self.right_arm_indices,limbconfig): q[i] = v def get_limb_config(self,q,limb): """Helper: Gets the 7-DOF configuration of the given limb given a full-robot configuration q""" qlimb = [0.0]*len(self.left_arm_indices) if limb=='left': for (i,j) in enumerate(self.left_arm_indices): qlimb[i] = q[j] else: for (i,j) in enumerate(self.right_arm_indices): qlimb[i] = q[j] return qlimb def check_collision_free(self,limb,exclude=None,verbose=False): """Checks whether the given limb is collision free at the robot's current configuration""" exclude = exclude or [] armfilter = None if limb=='left': collindices = set(baxter.left_arm_geometry_indices+baxter.left_hand_geometry_indices) handindices = baxter.left_hand_geometry_indices else: collindices = set(baxter.right_arm_geometry_indices+baxter.right_hand_geometry_indices) handindices = baxter.right_hand_geometry_indices armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) #check with objects in world model for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): #ignore hand self collisions if isinstance(o1[0],RobotModelLink) and o1[0].index in handindices and isinstance(o2[0],RobotModelLink) and o2[0].index in handindices: continue if o1[0].getID() not in exclude and o2[0].getID() not in exclude: if o1[1].collides(o2[1]): if verbose: print "Collision between",o1[0].getName(),o2[0].getName() return False return True def check_collision_free_with_object(self,limb,object,grasp): """Checks whether the given limb, holding an object at the given grasp, is collision free at the robot's current configuration""" objToGripperXForm = se3.mul(baxter.left_gripper_center_xform,se3.inv(grasp)) #assume robot configuration is updated if limb=='left': gripperLink = self.robot.getLink(baxter.left_gripper_link_name) else: gripperLink = self.robot.getLink(baxter.right_gripper_link_name) if not isinstance(object,list): if object==None: object = [] else: object = [object] Tgripper = gripperLink.getTransform() Tobj = se3.mul(Tgripper,objToGripperXForm) for o in object: o.setTransform(*Tobj) excludeids = [ o.getID() for o in object] if not self.check_collision_free(limb,exclude=excludeids): #print 'Limb is not collision free' return False for t in xrange(self.world.numRigidObjects()): other = self.world.rigidObject(t) if(other.getID() not in excludeids): if any(other.geometry().collides(o.geometry()) for o in object): #visualization.debug(self.world) #print "Held object-shelf collision" return False return True def check_limb_collision_free(self,limb,limbconfig): """Checks whether the given 7-DOF limb configuration is collision free, keeping the rest of self.robot fixed.""" q = self.robot.getConfig() self.set_limb_config(limb,limbconfig,q) self.robot.setConfig(q) return self.check_collision_free(limb) 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(self,start,goal,order=['left','right'],Cspace='Normal',argsTuple=None,extra_feasibility_tests=[]): """Plans a motion for the robot to move from configuration start to configuration goal. By default, moves the left arm first, then the right. To move the right first, set the 'order' argument to ['right','left']""" limbstart = {} limbgoal = {} for l in ['left','right']: limbstart[l] = self.get_limb_config(start,l) limbgoal[l] = self.get_limb_config(goal,l) path = [start] curconfig = start[:] for l in order: diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l])) if diff > 1e-8: #print "Planning for limb",l #print " Euclidean distance:",math.sqrt(diff) self.robot.setConfig(curconfig) #do the limb planning if(Cspace == 'scoop'): limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],Cspace='scoop',extra_feasibility_tests=extra_feasibility_tests) elif(Cspace == 'height'): limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],Cspace='height',argsTuple=argsTuple,extra_feasibility_tests=extra_feasibility_tests) elif(Cspace == 'xrestrictNobj'): limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],Cspace='xrestrict',argsTuple=argsTuple,extra_feasibility_tests=extra_feasibility_tests) else: limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],extra_feasibility_tests=extra_feasibility_tests) if limbpath == False: #print " Failed to plan for limb",l return None #print " Planned successfully for limb",l #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(l,qlimb,q) path.append(q) self.set_limb_config(l,limbgoal[l],curconfig) return path 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 plan_transfer(self,start,goal,limb,heldobject,grasp,type='Normal',xrestrict=None,extra_feasibility_tests=[]): """Plans a motion for the robot to move from configuration start to configuration goal while holding the given object.""" limbstart = self.get_limb_config(start,limb) limbgoal = self.get_limb_config(goal,limb) path = [start] #print "Planning transfer path for limb",limb self.robot.setConfig(start) #do the limb planning limbpath = self.plan_limb_transfer(limb,limbstart,limbgoal,heldobject,grasp,type,xrestrict,extra_feasibility_tests=extra_feasibility_tests) if limbpath == False: #print " Failed to plan transfer path for limb",limb return None #print " Planned transfer path successfully for limb",limb #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(limb,qlimb,q) path.append(q) return path def updateWorld(self,new_world): self.world = new_world self.collider = WorldCollider(new_world) def check_collision_free_with_object_base(self,limb,object,grasp): """Checks whether the given limb, holding an object at the given grasp, is collision free at the robot's current configuration""" objToGripperXForm = se3.mul(baxter.left_gripper_center_xform,se3.inv(grasp)) #assume robot configuration is updated if limb=='left': gripperLink = self.robot.getLink(baxter.left_gripper_link_name) else: gripperLink = self.robot.getLink(baxter.right_gripper_link_name) if not isinstance(object,list): if object==None: object = [] else: object = [object] Tgripper = gripperLink.getTransform() Tobj = se3.mul(Tgripper,objToGripperXForm) for o in object: o.setTransform(*Tobj) excludeids = [ o.getID() for o in object] if not self.check_collision_free_hand(limb,exclude=excludeids): #print 'Limb is not collision free' return False #if not self.check_collision_free(limb,exclude=excludeids): # #print 'Limb is not collision free' # return False for t in xrange(self.world.numRigidObjects()): other = self.world.rigidObject(t) if(other.getID() not in excludeids): if any(other.geometry().collides(o.geometry()) for o in object): #visualization.debug(self.world) #print "Held object-shelf collision" return False return True def check_collision_free_hand(self,limb,exclude=None,verbose=False): """Checks whether the given limb is collision free at the robot's current configuration""" exclude = exclude or [] armfilter = None if limb=='left': collindices = set(baxter.left_arm_geometry_indices+baxter.left_hand_geometry_indices) handindices = baxter.left_hand_geometry_indices else: collindices = set(baxter.right_arm_geometry_indices+baxter.right_hand_geometry_indices) handindices = baxter.right_hand_geometry_indices #add in the base collindices.add(0) collindices.add(1) collindices.add(2) collindices.add(3) collindices.add(4) collindices.add(5) armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) #check with objects in world model for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): #ignore hand self collisions if isinstance(o1[0],RobotModelLink) and o1[0].index in handindices and isinstance(o2[0],RobotModelLink) and o2[0].index in handindices: continue if o1[0].getID() not in exclude and o2[0].getID() not in exclude: if o1[1].collides(o2[1]): if verbose: print "Collision between",o1[0].getName(),o2[0].getName() return False if(not isinstance(o1[0],RobotModelLink) and o2[0].index not in handindices): print 'checking object with base' if o1[1].collides(o2[1]): if verbose: print "Collision between",o1[0].getName(),o2[0].getName() return False if(not isinstance(o2[0],RobotModelLink) and o1[0].index not in handindices): if o1[1].collides(o2[1]): if verbose: print "Collision between",o1[0].getName(),o2[0].getName() return False return True
class LimbPlanner: """Much of your code for HW4 will go here. Attributes: - world: the WorldModel - robot: the RobotModel - knowledge: the KnowledgeBase objcet - collider: a WorldCollider object (see the klampt.robotcollide module) """ 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.link(left_camera_link_name) self.right_camera_link = self.robot.link(right_camera_link_name) self.left_gripper_link = self.robot.link(left_gripper_link_name) self.right_gripper_link = self.robot.link(right_gripper_link_name) self.left_arm_links = [self.robot.link(i) for i in left_arm_link_names] self.right_arm_links = [self.robot.link(i) for i in right_arm_link_names] id_to_index = dict([(self.robot.link(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.dynamic_objects = [] def set_limb_config(self,limb,limbconfig,q): """Helper: Sets the 7-DOF configuration of the given limb in q. Other DOFs are not touched.""" assert len(q) == self.robot.numLinks() if limb=='left': for (i,v) in zip(self.left_arm_indices,limbconfig): q[i] = v else: for (i,v) in zip(self.right_arm_indices,limbconfig): q[i] = v def get_limb_config(self,q,limb): """Helper: Gets the 7-DOF configuration of the given limb given a full-robot configuration q""" qlimb = [0.0]*len(self.left_arm_indices) if limb=='left': for (i,j) in enumerate(self.left_arm_indices): qlimb[i] = q[j] else: for (i,j) in enumerate(self.right_arm_indices): qlimb[i] = q[j] return qlimb def check_collision_free(self,limb): """Checks whether the given limb is collision free at the robot's current configuration""" armfilter = None if limb=='left': collindices = set(left_arm_geometry_indices+left_hand_geometry_indices) else: collindices = set(right_arm_geometry_indices+right_hand_geometry_indices) armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) #check with objects in world model for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): if o1[1].collides(o2[1]): # print "Collision between",o1[0].getName(),o2[0].getName() return False for obj in self.dynamic_objects: assert obj.info.geometry != None for link in collindices: if self.robot.link(link).geometry().collides(obj.info.geometry): # NOTE: Uncomment this line to show collision warnings # print "Collision between link",self.robot.link(link).getName()," and dynamic object" return False return True def check_collision_free_with_object(self,limb,objectGeom,grasp): """Checks whether the given limb, holding an object at the given grasp, is collision free at the robot's current configuration""" if not self.check_collision_free(limb): return False objToGripperXForm = se3.mul(left_gripper_center_xform,se3.inv(grasp.grasp_xform)) #assume robot configuration is updated if limb=='left': gripperLink = self.robot.link(left_gripper_link_name) else: gripperLink = self.robot.link(right_gripper_link_name) Tgripper = gripperLink.getTransform(); Tobj = se3.mul(Tgripper,objToGripperXForm) objectGeom.setCurrentTransform(*Tobj) for t in xrange(self.world.numTerrains()): if self.world.terrain(t).geometry().collides(objectGeom): # print "Held object-shelf collision" return False for o in self.dynamic_objects: if o.info.geometry.collides(objectGeom): # print "Held object-object collision" return False return True def rebuild_dynamic_objects(self): self.dynamic_objects = [] #check with objects in knowledge for (k,objList) in self.knowledge.bin_contents.iteritems(): if objList == None: #not sensed continue for item in objList: assert item.info.geometry != None item.info.geometry.setCurrentTransform(*item.xform) self.dynamic_objects.append(item) return def check_limb_collision_free(self,limb,limbconfig): """Checks whether the given 7-DOF limb configuration is collision free, keeping the rest of self.robot fixed.""" q = self.robot.getConfig() self.set_limb_config(limb,limbconfig,q) self.robot.setConfig(q) return self.check_collision_free(limb) 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(self,start,goal,order=['left','right'],printer=True): """Plans a motion for the robot to move from configuration start to configuration goal. By default, moves the left arm first, then the right. To move the right first, set the 'order' argument to ['right','left']""" limbstart = {} limbgoal = {} for l in ['left','right']: limbstart[l] = self.get_limb_config(start,l) limbgoal[l] = self.get_limb_config(goal,l) path = [start] curconfig = start[:] for l in order: diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l])) if diff > 1e-8: if printer: print "< Planning for limb",l,">" print " Euclidean distance:",math.sqrt(diff) self.robot.setConfig(curconfig) #do the limb planning limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer) if limbpath == 1 or limbpath == 2 or limbpath == False: if printer: print " Failed to plan for limb",l,"\n" return limbpath if printer: print " Planned successfully for limb",l, "\n" #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(l,qlimb,q) path.append(q) self.set_limb_config(l,limbgoal[l],curconfig) return path 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') # 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: print " Found a path on iteration",iters,'/',maxPlanIters 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_transfer(self,start,goal,limb,heldobject,grasp): """Plans a motion for the robot to move from configuration start to configuration goal while holding the given object.""" limbstart = self.get_limb_config(start,limb) limbgoal = self.get_limb_config(goal,limb) path = [start] print "Planning transfer path for limb",limb self.robot.setConfig(start) #do the limb planning limbpath = self.plan_limb_transfer(limb,limbstart,limbgoal,heldobject,grasp) if limbpath == False: print " Failed to plan transfer path for limb",limb return None print " Planned transfer path successfully for limb",limb #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(limb,qlimb,q) path.append(q) return path
class LimbPlanner: """Much of your code for HW4 will go here. Attributes: - world: the WorldModel - robot: the RobotModel - knowledge: the KnowledgeBase objcet - collider: a WorldCollider object (see the klampt.robotcollide module) """ 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.link(left_camera_link_name) self.right_camera_link = self.robot.link(right_camera_link_name) self.left_gripper_link = self.robot.link(left_gripper_link_name) self.right_gripper_link = self.robot.link(right_gripper_link_name) self.left_arm_links = [self.robot.link(i) for i in left_arm_link_names] self.right_arm_links = [self.robot.link(i) for i in right_arm_link_names] id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())]) self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices self.right_arm_indices = right_arm_geometry_indices + right_hand_geometry_indices self.dynamic_objects = [] # NOTE: added from lab3e.py self.roadmap = ([],[]) self.limb_indices = [] self.pathToDraw = [] self.colMargin = 0 self.activeLimb = None def set_limb_config(self,limb,limbconfig,q): """Helper: Sets the 7-DOF configuration of the given limb in q. Other DOFs are not touched.""" assert len(q) == self.robot.numLinks() if limb=='left': for (i,v) in zip(self.left_arm_indices,limbconfig): q[i] = v else: for (i,v) in zip(self.right_arm_indices,limbconfig): q[i] = v def get_limb_config(self,q,limb): """Helper: Gets the 7-DOF configuration of the given limb given a full-robot configuration q""" qlimb = [0.0]*len(self.left_arm_indices) if limb=='left': qlimb = [0.0]*len(self.left_arm_indices) for (i,j) in enumerate(self.left_arm_indices): qlimb[i] = q[j] else: qlimb = [0.0]*len(self.right_arm_indices) for (i,j) in enumerate(self.right_arm_indices): qlimb[i] = q[j] return qlimb def check_collision_free(self,limb,ignoreColShelfSpatula=False): """Checks whether the given limb is collision free at the robot's current configuration""" armfilter = None if limb=='left': # if ignoreColShelfSpatula: # collindices = set(left_arm_geometry_indices) # else: # collindices = set(left_arm_geometry_indices+left_hand_geometry_indices) collindices = set(left_arm_geometry_indices+left_hand_geometry_indices) else: collindices = set(right_arm_geometry_indices+right_hand_geometry_indices) armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) ignoreList = ["Amazon_Picking_Shelf", "bin_"] ignoreList = '\t'.join(ignoreList) spatulaIgnoreList = [55,56,57] # if ignoreColShelfSpatula: # spatulaIgnoreList = [54,55,56,57] #check with objects in world model for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): # NOTE: what is bb_reject?? # print "Collision Test: Collision between",o1[0].getName(),o2[0].getName() # if ignoreColShelfSpatula: # print o1[0].getName()[0:4] if o1[0].getName()[0:3] in ignoreList and o2[0].index in spatulaIgnoreList: # print "ignoring collision between shelf and spautla" continue if o2[0].getName()[0:3] in ignoreList and o1[0].index in spatulaIgnoreList: # print "ignoring collision between shelf and spautla" continue if o1[0].getName()[0:3] in ignoreList: for obj in self.dynamic_objects: assert obj.info.geometry != None if o1[1].collides(obj.info.geometry): continue if o2[0].getName()[0:3] in ignoreList: for obj in self.dynamic_objects: assert obj.info.geometry != None if o1[1].collides(obj.info.geometry): continue if o1[1].collides(o2[1]): print "Collision between",o1[0].getName(),o2[0].getName() return False # for obj in self.dynamic_objects: # # print "checking collision with objects" # # print obj.info.geometry # assert obj.info.geometry != None # for link in collindices: # if self.robot.link(link).geometry().collides(obj.info.geometry): # print "Collision between link",self.robot.link(link).getName()," and dynamic object" # return False # for link in collindices: # # print "Collision Test: Collision between",self.robot.link(link).getName(),"shelf" # shelfGeometry = self.world.terrain(0).geometry() # linkGeometry = self.robot.link(link).geometry() # if shelfGeometry.collides(linkGeometry): # print "link #",link,"collides with terrain" # return False # print self.robot.getConfig() return True def rebuild_dynamic_objects(self): self.dynamic_objects = [] #check with objects in knowledge for (k,objList) in self.knowledge.bin_contents.iteritems(): if objList == None: #not sensed continue for item in objList: assert item.info.geometry != None item.info.geometry.setCurrentTransform(*item.xform) self.dynamic_objects.append(item) return def check_limb_collision_free(self,limb,limbconfig,ignoreColShelfSpatula=False): """Checks whether the given 7-DOF limb configuration is collision free, keeping the rest of self.robot fixed.""" q = self.robot.getConfig() self.set_limb_config(limb,limbconfig,q) self.robot.setConfig(q) return self.check_collision_free(limb,ignoreColShelfSpatula=ignoreColShelfSpatula) 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 def plan(self,start,goal,limb,printer=True, iks = None, ignoreColShelfSpatula = False): """Plans a motion for the robot to move from configuration start to configuration goal. By default, moves the left arm first, then the right. To move the right first, set the 'order' argument to ['right','left']""" limbstart = {} limbgoal = {} # for l in ['left','right']: l =limb limbstart[l] = self.get_limb_config(start,l) limbgoal[l] = self.get_limb_config(goal,l) path = [start] curconfig = start[:] # for l in order: # diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l])) # if diff > 1e-8: # if printer: # print "< Planning for limb",l,">" # print " Euclidean distance:",math.sqrt(diff) # self.robot.setConfig(curconfig) # #do the limb planning # limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks) # if limbpath == 1 or limbpath == 2 or limbpath == False: # if printer: # print " Failed to plan for limb",l,"\n" # return limbpath # if printer: # print " Planned successfully for limb",l, "\n" # #concatenate whole body path # for qlimb in limbpath[1:]: # q = path[-1][:] # self.set_limb_config(l,qlimb,q) # path.append(q) # self.set_limb_config(l,limbgoal[l],curconfig) diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l])) if diff > 1e-8: if printer: print "< Planning for limb",l,">" # print " Euclidean distance:",math.sqrt(diff) self.robot.setConfig(curconfig) #do the limb planning if iks == None: limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks, ignoreColShelfSpatula = ignoreColShelfSpatula) else: limbpath = self.plan_limb(l,limbstart[l],limbgoal[l],printer=printer, iks=iks) if limbpath == 1 or limbpath == 2 or limbpath == False: if printer: print " Failed to plan for limb",l,"\n" return limbpath if printer: print " Planned successfully for limb",l, "\n" #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(l,qlimb,q) path.append(q) self.set_limb_config(l,limbgoal[l],curconfig) return path
class LimbPlanner: """Much of your code for HW4 will go here. Attributes: - world: the WorldModel - robot: the RobotModel - collider: a WorldCollider object (see the klampt.robotcollide module) """ def __init__(self,world): self.world = world self.robot = world.robot(0) self.collider = WorldCollider(world) self.roadmap = ([],[]) self.dynamic_objects = [] def check_collision_free(self,q): self.robot.setConfig(q) armfilter = None collindices = set(range(0,self.robot.numLinks())) #print "collindices", collindices armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) #check with objects in world model (includes the plane) for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): # print "Collision Test: Collision between",o1[0].getName(),o2[0].getName() #print "Collision Test: Collision for ",o2[0].getName() #print o1[0].index, o2[0].index if o1[1].collides(o2[1]): # print "Collision between",o1[0].getName(),o2[0].getName() # print "Collision between",o1[0].index,o2[0].index return False for obj in self.dynamic_objects: # print "checking collision with objects:", obj.getName() # print obj.info.geometry assert obj.geometry() != None for link in collindices: if self.robot.link(link).geometry().collides(obj.geometry()): # print "Collision between link",self.robot.link(link).getName()," and dynamic object" return False return True def rebuild_dynamic_objects(self): item = self.world.rigidObject(0) assert item.geometry() != None # item.info.geometry.setCurrentTransform(*item.xform) self.dynamic_objects.append(item) return def plan_limb(self,start,goal, 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) if iks != None: print "Initializing ClosedLoopCSPace" cspace = ClosedLoopCSpaceTest(self,limb,iks) if not cspace.feasible(start): print " Start configuration is infeasible!" return 1 if not cspace.feasible(goal): print " Goal configuration is infeasible!" return 2 # MotionPlan.setOptions(type="rrt", perturbationRadius = 1, connectionThreshold=2, bidirectional = True, shortcut = True, restart=True) # plan = MotionPlan(cspace,'sbl') plan = MotionPlan(cspace) plan.setEndpoints(start,goal) # maxPlanIters = 20 # maxSmoothIters = 100 maxPlanIters = 100 maxSmoothIters = 1000 # print "start =",start,"\n","goal = ", goal 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 "iter:",iters,"--",len(V),"feasible milestones sampled,",len(E),"edges connected" # print iters,"/V",len(V),"/E",len(E), # print len(V),".", 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(self,start,goal,printer=True, iks = None, ignoreColShelfSpatula = True): """Plans a motion for the robot to move from configuration start to configuration goal. By default, moves the left arm first, then the right. To move the right first, set the 'order' argument to ['right','left']""" path = [start] curconfig = start[:] diff = sum((a-b)**2 for a,b in zip(start,goal)) if diff > 1e-8: if printer: print "< Planning ... >" # print " Euclidean distance:",math.sqrt(diff) self.robot.setConfig(curconfig) # #do the limb planning # if not ignoreColShelfSpatula: # self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices + [55,56,57] # else: # self.left_arm_indices = left_arm_geometry_indices + left_hand_geometry_indices # print " Left arm links", self.left_arm_indices # if iks == None: path = self.plan_limb(start,goal,printer=printer) # else: # trajectory = self.plan_closedLoop(start,goal,iks=iks) # return trajectory if path == 1 or path == 2 or path == False: if printer: print " Failed to plan (type =", path,")", "\n" return None if printer: print " Planned successfully\n" #concatenate whole body path # for qlimb in limbpath[1:]: # q = path[-1][:] # self.set_limb_config(l,qlimb,q) # path.append(q) # self.set_limb_config(l,limbgoal[l],curconfig) return path 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
class LimbPlanner: """Much of your code for HW4 will go here. Attributes: - world: the WorldModel - robot: the RobotModel - knowledge: the KnowledgeBase objcet - collider: a WorldCollider object (see the klampt.robotcollide module) """ def __init__(self,world, vacuumPc): self.world = world self.robot = world.robot(0) self.collider = WorldCollider(world) self.vacuumPc = vacuumPc #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_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] 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.dynamic_objects = [] def set_limb_config(self,limb,limbconfig,q): """Helper: Sets the 7-DOF configuration of the given limb in q. Other DOFs are not touched.""" assert len(q) == self.robot.numLinks() if limb=='left': for (i,v) in zip(self.left_arm_indices,limbconfig): q[i] = v else: for (i,v) in zip(self.right_arm_indices,limbconfig): q[i] = v def get_limb_config(self,q,limb): """Helper: Gets the 7-DOF configuration of the given limb given a full-robot configuration q""" qlimb = [0.0]*len(self.left_arm_indices) if limb=='left': for (i,j) in enumerate(self.left_arm_indices): qlimb[i] = q[j] else: for (i,j) in enumerate(self.right_arm_indices): qlimb[i] = q[j] return qlimb def check_collision_free(self,limb): """Checks whether the given limb is collision free at the robot's current configuration""" armfilter = None left_arm_geometry_indices = [15,16,17,18,19,21,22,23,30,31] right_arm_geometry_indices = [35,36,37,38,39,41,42,43,50,51] left_hand_geometry_indices = [54,55,56] right_hand_geometry_indices = [57,58,59] if limb=='left': collindices = set(left_arm_geometry_indices+left_hand_geometry_indices) else: collindices = set(right_arm_geometry_indices+right_hand_geometry_indices) armfilter = lambda x:isinstance(x,RobotModelLink) and (x.index in collindices) #check with objects in world model for o1,o2 in self.collider.collisionTests(armfilter,lambda x:True): if o1[1].collides(o2[1]): #print "Collision between",o1[0].getName(),o2[0].getName() return False for obj in self.dynamic_objects: assert obj.info.geometry != None for link in collindices: if self.robot.getLink(link).geometry().collides(obj.info.geometry): print "Collision between link",self.robot.getLink(link).getName()," and dynamic object" return False for i in xrange(self.world.numRigidObjects()): o = self.world.rigidObject(i) g = o.geometry() if g != None and g.type()!="": if self.vacuumPc.withinDistance(g, .03): return False return True def rebuild_dynamic_objects(self): pass # self.dynamic_objects = [] # #check with objects in knowledge # for (k,objList) in self.knowledge.bin_contents.iteritems(): # if objList == None: # #not sensed # continue # for item in objList: # assert item.info.geometry != None # item.info.geometry.setCurrentTransform(*item.xform) # self.dynamic_objects.append(item) # return def check_limb_collision_free(self,limb,limbconfig): """Checks whether the given 7-DOF limb configuration is collision free, keeping the rest of self.robot fixed.""" q = self.robot.getConfig() self.set_limb_config(limb,limbconfig,q) self.robot.setConfig(q) return self.check_collision_free(limb) 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""" self.rebuild_dynamic_objects() cspace = LimbCSpace(self,limb) cspace.setup() 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() valid_path = True if path != None: for milestone in path: if not cspace.feasible(milestone): valid_path = False if path != None and valid_path: 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(self,start,goal,order=['left','right']): """Plans a motion for the robot to move from configuration start to configuration goal. By default, moves the left arm first, then the right. To move the right first, set the 'order' argument to ['right','left']""" limbstart = {} limbgoal = {} for l in ['left','right']: limbstart[l] = self.get_limb_config(start,l) limbgoal[l] = self.get_limb_config(goal,l) path = [start] curconfig = start[:] for l in order: diff = sum((a-b)**2 for a,b in zip(limbstart[l],limbgoal[l])) if diff > 1e-8: print "Planning for limb",l print " Euclidean distance:",math.sqrt(diff) self.robot.setConfig(curconfig) #do the limb planning limbpath = self.plan_limb(l,limbstart[l],limbgoal[l]) if limbpath == False: print " Failed to plan for limb",l return None print " Planned successfully for limb",l #concatenate whole body path for qlimb in limbpath[1:]: q = path[-1][:] self.set_limb_config(l,qlimb,q) path.append(q) self.set_limb_config(l,limbgoal[l],curconfig) return path