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) self.solutions = 0 self.collisions = 0 #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] 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] def reinit_collider(self): self.collider = WorldCollider(self.world) 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""" self.solutions += 1 # Use collider to determine collisions and return false if one has been found selfCollisionIterator = self.collider.robotSelfCollisions(self.robot) for collision in selfCollisionIterator: print "self collision detected", collision[0].index, collision[1].index self.collisions += 1 return False # Get list of all terrain indices terrainIds = [] for i in range(self.world.numTerrains()): terrainIds.append(self.world.terrain(i).getID()) # Create filter based on indices terrainFilter = lambda x: x.getID() in terrainIds # Create filter for robot links if limb == 'left': robotLinkFilter = lambda x: x.index in left_arm_geometry_indices else: robotLinkFilter = lambda x: x.index in right_arm_geometry_indices # Check terrain collisions for collision in self.collider.collisions(filter1=terrainFilter, filter2=robotLinkFilter): print "terrain robot collision detected", collision[0].index, collision[1].index return False # Get list of all object IDs objectIDs = [] for i in range(self.world.numRigidObjects()): objectIDs.append(self.world.rigidObject(i).getID()) # Create filter for object IDs objectFilter = lambda x: x.getID() in objectIDs # Check object and robot link collisions for collision in self.collider.collisions(filter1=objectFilter, filter2=robotLinkFilter): print "object robot collision detected", collision[0].index, collision[1].index 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): """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(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: if limbstart[l] != limbgoal[l]: 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
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(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] 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] 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""" for collision in self.collider.robotSelfCollisions(self.robot): 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): """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. return [start,goal] 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: if limbstart[l] != limbgoal[l]: 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
class PickingController: """Maintains the robot's knowledge base and internal state. Most of your code will go here. Members include: - knowledge: a KnowledgeBase object - planner: an LimbPlanner object, which *you will implement and use* - state: either 'ready', or 'holding' - configuration: the robot's current configuration - active_limb: the limb currently active, either holding or viewing a state - current_bin: the name of the bin where the camera is viewing or the gripper is located - held_object: the held object, if one is held, or None otherwise External modules can call viewBinAction(), graspAction(), ungraspAction(), and placeInOrderBinAction() """ def __init__(self,world,robotController): self.world = world self.robot = world.robot(0) self.controller = robotController self.knowledge = KnowledgeBase() self.planner = LimbPlanner(self.world,self.knowledge) self.state = 'ready' self.active_limb = None self.active_grasp = None self.current_bin = None self.held_object = None #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] 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.collider = WorldCollider(world) def waitForMove(self,timeout = None, pollRate = 0.5): """Waits for the move to complete, or timeout seconds is elapsed, before terminating.""" iters = 0 t = 0 while self.controller.isMoving(): if iters % 10 == 0: print "Waiting for move to complete..." time.sleep(pollRate) t += pollRate if timeout != None and t > timeout: return False iters += 1 return True def viewBinAction(self,b): self.waitForMove() if self.state != 'ready': print "Already holding an object, can't move to bin" return False else: if b in apc.bin_names: if self.move_camera_to_bin(b): self.waitForMove() self.current_bin = b run_perception_on_bin(self.knowledge,b) print "Sensed bin",b,"with camera",self.active_limb else: print "Move to bin",b,"failed" return False else: print "Invalid bin",b return False return True def graspAction(self): self.waitForMove() self.controller.commandGripper(self.active_limb,[1]) self.waitForMove() if self.current_bin == None: print "Not located at a bin" return False elif self.state != 'ready': print "Already holding an object, can't grasp another" return False elif len(self.knowledge.bin_contents[self.current_bin])==0: print "The current bin is empty" return False else: if self.move_to_grasp_object(self.knowledge.bin_contents[self.current_bin][0]): self.waitForMove() #now close the gripper self.controller.commandGripper(self.active_limb,self.active_grasp.gripper_close_command) self.waitForMove() self.held_object = self.knowledge.bin_contents[self.current_bin].pop(0) self.state = 'holding' print "Holding object",self.held_object.info.name,"in hand",self.active_limb return True else: print "Grasp failed" return False def ungraspAction(self): self.waitForMove() if self.state != 'holding': print "Not holding an object" return False else: if self.move_to_ungrasp_object(self.held_object): self.waitForMove() #now open the gripper self.controller.commandGripper(self.active_limb,self.active_grasp.gripper_open_command) self.waitForMove() print "Object",self.held_object.info.name,"placed back in bin" self.knowledge.bin_contents[self.current_bin].append(self.held_object) self.state = 'ready' self.held_object = None return True else: print "Ungrasp failed" return False def placeInOrderBinAction(self): self.waitForMove() if self.state != 'holding': print "Not holding an object" else: if self.move_to_order_bin(self.held_object): self.waitForMove() #now open the gripper self.controller.commandGripper(self.active_limb,self.active_grasp.gripper_open_command) self.waitForMove() print "Successfully placed",self.held_object.info.name,"into order bin" self.knowledge.order_bin_contents.append(self.held_object) self.held_object.xform = None self.held_object.bin_name = 'order_bin' self.state = 'ready' self.held_object = None return True else: print "Move to order bin failed" return False def fulfillOrderAction(self,objectList): """Given a list of objects to be put in the order bin, run until completed.""" remainingObjects = objectList for b in apc.bin_names: if self.knowledge.bin_contents[b]==None: if not self.viewBinAction(b): print "Could not view bin",b continue donextbin = False while any(o.info.name in remainingObjects for o in self.knowledge.bin_contents[b]) and not donextbin: #pick up and put down objects until you are holding one that is in the remainingObjects list if not self.graspAction(): print "Error grasping object" donextbin = True break while not donextbin and (self.held_object == None or self.held_object.info.name not in remainingObjects): #cycle through objects by putting down and picking up the next object if not self.ungraspAction(): print "Error putting down object" return False if not self.graspAction(): print "Error grasping object" donextbin = True break obj = self.held_object if self.placeInOrderBinAction(): remainingObjects.remove(obj.info.name) else: print "Error putting object into order bin" return False if len(remainingObjects)==0: return True print "These items are remaining from the order:",remainingObjects return False def randomize_limb_position(self,limb,range=None): """Helper: randomizes the limb configuration in self.robot. limb can be 'left' or 'right'. If range is provided, then this samples in a range around the current commanded config""" qmin,qmax = self.robot.getJointLimits() if range == None: q = baxter_rest_config[:] if limb == 'left': for j in self.left_arm_indices: q[j] = random.uniform(qmin[j],qmax[j]) else: for j in self.right_arm_indices: q[j] = random.uniform(qmin[j],qmax[j]) self.robot.setConfig(q) else: q = self.controller.getCommandedConfig() if limb == 'left': for j in self.left_arm_indices: q[j] = max(qmin[j],min(qmax[j],random.uniform(q[j]-range,q[j]+range))) else: for j in self.right_arm_indices: q[j] = max(qmin[j],min(qmax[j],random.uniform(q[j]-range,q[j]+range))) self.robot.setConfig(q) return def move_camera_to_bin(self,bin_name): """Starts a motion so the camera has a viewpoint that observes bin_name. Will also change self.active_limb to the appropriate limb. If successful, sends the motion to the low-level controller and returns True. Otherwise, does not modify the low-level controller and returns False. """ world_offset = self.knowledge.bin_vantage_point(bin_name) #place +z in the +x axis, y in the +z axis, and x in the -y axis left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset) right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset) qcmd = self.controller.getCommandedConfig() collisionTries = 0 for i in range(100): if random.random() < 0.5: oldConfig = self.robot.getConfig() if i == 0: self.robot.setConfig(qcmd) else: self.randomize_limb_position('left') if ik.solve(left_goal): #TODO: plan a path # check that this solution is self collision free # we call the self collision function of WorldCollider # and see how many items the generator returns. If it is 0 # then we don't collide (if it is not zero, we don't care what # they are, we throw the solution away and retry with a different # initial arm configuration). collisionTries += 1 collisions = self.collider.robotSelfCollisions(robot = self.robot) numCols = 0 for cols in collisions: numCols += 1 self.controller.setMilestone(self.robot.getConfig()) if numCols == 0: self.controller.setMilestone(self.robot.getConfig()) self.active_limb = 'left' print "Found self collision free IK solution after " + str(collisionTries) + " tries" return True else: if i == 0: self.robot.setConfig(qcmd) else: self.randomize_limb_position('right') if ik.solve(right_goal): #TODO: plan a path # check that this solution is self collision free # we call the self collision function of WorldCollider # and see how many items the generator returns. If it is 0 # then we don't collide (if it is not zero, we don't care what # they are, we throw the solution away and retry with a different # initial arm configuration). collisionTries += 1 collisions = self.collider.robotSelfCollisions(robot = self.robot) numCols = 0 for cols in collisions: numCols += 1 if numCols == 0: self.controller.setMilestone(self.robot.getConfig()) self.active_limb = 'right' print "Found self collision free IK solution after " + str(collisionTries) + " tries" return True return False def move_to_grasp_object(self,object): """Sets the robot's configuration so the gripper grasps object at one of its potential grasp locations. Might change self.active_limb to the appropriate limb. Must change self.active_grasp to the selected grasp. If successful, sends the motion to the low-level controller and returns True. Otherwise, does not modify the low-level controller and returns False. """ collisionTries = 0 grasps = self.knowledge.grasp_xforms(object) qmin,qmax = self.robot.getJointLimits() qcmd = self.controller.getCommandedConfig() #phase 1: init IK from the commanded config, search among grasps for (grasp,gxform) in grasps: if self.active_limb == 'left': Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform)) goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1]) else: Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform)) goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1]) self.robot.setConfig(qcmd) if ik.solve(goal): # check that this solution is self collision free # we call the self collision function of WorldCollider # and see how many items the generator returns. If it is 0 # then we don't collide (if it is not zero, we don't care what # they are, we throw the solution away and retry with a different # initial arm configuration). collisionTries += 1 collisions = self.collider.robotSelfCollisions(robot = self.robot) numCols = 0 for cols in collisions: numCols += 1 if numCols == 0: self.controller.setMilestone(self.robot.getConfig()) self.active_grasp = grasp print "Found self collision free IK solution after " + str(collisionTries) + " tries" return True #Phase 2: that didn't work, now try random sampling for i in range(100): #pick a config at random self.randomize_limb_position(self.active_limb) #pick a grasp at random (grasp,gxform) = random.choice(grasps) if self.active_limb == 'left': Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform)) goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1]) else: Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform)) goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1]) if ik.solve(goal): #TODO: plan a path # check that this solution is self collision free # we call the self collision function of WorldCollider # and see how many items the generator returns. If it is 0 # then we don't collide (if it is not zero, we don't care what # they are, we throw the solution away and retry with a different # initial arm configuration). collisionTries += 1 collisions = self.collider.robotSelfCollisions(robot = self.robot) numCols = 0 for cols in collisions: numCols += 1 self.controller.setMilestone(self.robot.getConfig()) if numCols == 0: self.controller.setMilestone(self.robot.getConfig()) self.active_grasp = grasp print "Found self collision free IK solution after " + str(collisionTries) + " tries" return True return False def move_to_ungrasp_object(self,object): """Sets the robot's configuration so the gripper ungrasps the object. If successful, sends the motion to the low-level controller and returns True. Otherwise, does not modify the low-level controller and returns False. """ assert len(object.info.grasps) > 0,"Object doesn't define any grasps" return True def move_to_order_bin(self,object): """Sets the robot's configuration so the gripper is over the order bin If successful, sends the motion to the low-level controller and returns True. Otherwise, does not modify the low-level controller and returns False. """ collisionTries = 0 left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1]) right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1]) qcmd = self.controller.getCommandedConfig() for i in range(100): if self.active_limb == 'left': goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target) else: goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target) #set IK solver initial configuration if i==0: self.robot.setConfig(qcmd) else: self.randomize_limb_position(self.active_limb) #solve if ik.solve(goal,tol=0.1): #TODO: plan a path # check that this solution is self collision free # we call the self collision function of WorldCollider # and see how many items the generator returns. If it is 0 # then we don't collide (if it is not zero, we don't care what # they are, we throw the solution away and retry with a different # initial arm configuration). collisionTries += 1 collisions = self.collider.robotSelfCollisions(robot = self.robot) numCols = 0 for cols in collisions: numCols += 1 self.controller.setMilestone(self.robot.getConfig()) if numCols == 0: self.controller.setMilestone(self.robot.getConfig()) print "Found self collision free IK solution after " + str(collisionTries) + " tries" return True return False