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 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, world): GLWidgetProgram.__init__(self, "Manual poser") #start up the poser in the currently commanded configuration q = motion.robot.getKlamptCommandedPosition() world.robot(0).setConfig(q) self.world = world self.robot = world.robot(0) self.robotPoser = RobotPoser(world.robot(0)) self.addWidget(self.robotPoser) self.roadMap = ([], []) robot = world.robot(0) left_arm_link_names = [ 'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm', 'left_wrist' ] right_arm_link_names = [ 'right_upper_shoulder', 'right_lower_shoulder', 'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm', 'right_lower_forearm', 'right_wrist' ] self.left_arm_link_names = left_arm_link_names self.right_arm_link_names = right_arm_link_names missing_left_arm_names = [ 'left_upper_forearm_visual', 'left_upper_elbow_visual' ] missing_right_arm_names = [ 'right_upper_forearm_visual', 'right_upper_elbow_visual' ] #self.left_arm_link_names+=missing_left_arm_names #self.right_arm_link_names+=missing_right_arm_names self.left_arm_link_names = [ 'left_upper_forearm', 'left_lower_forearm', 'left_wrist', 'left_upper_forearm_visual', 'left_gripper:base' ] self.right_arm_link_names = [ 'right_upper_forearm', 'right_lower_forearm', 'right_wrist', 'right_upper_forearm_visual', 'right_gripper:base' ] self.left_arm_link_indices = [ robot.link(l).index for l in left_arm_link_names ] self.right_arm_link_indices = [ robot.link(l).index for l in right_arm_link_names ] self.firstTimeHack = True self.world.makeTerrain('terrain') self.subscribe() self.newPC = None self.collider = WorldCollider(self.world)
def getClosestConfig(self, robot, target, iterations, c, numsteps): cost = 9999 res = None start = robot.getConfig() #goal = ik.objective(robot.link("left_gripper:base"), local = [(0,0,0)], world = [target]) #goal = ik.objective(robot.link("left_wrist"), local = [(0,0,0)], world = [target]) s = IKSolver(robot) objective = IKObjective() objective.setFixedPoint( robot.link("left_gripper:base").getIndex(), (0, 0, 0), target) s.add(objective) s.setActiveDofs([12, 13, 14, 15, 16, 18, 19]) print "Active DOFs:", s.getActiveDofs() for i in range(iterations): #if ik.solve(goal): (ret, iters) = s.solve(100, 1e-4) if ret: end = robot.getConfig() print "*****************************", end qmin, qmax = robot.getJointLimits() #print qmin #print qmax flag = False for k in range(len(end)): if end[k] < qmin[k] or end[k] > qmax[k]: flag = True break if flag: start = self.perturb(start, 0.1) robot.setConfig(start) continue for j in xrange(numsteps + 1): u = float(j) / numsteps print "u is:", u q = robot.interpolate(end, start, u) #q = end print "interpolated q is:", q if not inCollision(WorldCollider(self.world), robot, q): newcost = vectorops.distance( q, end) + c * vectorops.distance(q, start) if newcost < cost: res = q cost = newcost break start = self.perturb(start, 0.1) robot.setConfig(start) print "res is:", res return res
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 __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 keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example toggles simulation / movie mode if c == 'h': print '[space]: send the current posed milestone' print 'q: clean quit' elif c == ' ': q = self.robotPoser.get() q0 = motion.robot.getKlamptCommandedPosition() t1 = time.time() #collisions = obstaclecollision(WorldCollider(self.world),self.world.robot(0),q0,q) collides = bisection(WorldCollider(self.world), self.world.robot(0), q0, q) print "Obstacle collision detection done in time", +time.time() - t1 exit(0) for i in range(self.world.robot(0).numLinks()): self.world.robot(0).link(i).appearance().setColor( 0.5, 0.5, 0.5, 1) #if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q): if collides: print "Invalid configuration, it self-collides" elif not self.firstTimeHack and collisions != None: #clear all links to gray for pairs in collisions: print "Link " + str( pairs[0].getIndex()) + " collides with obstacle" self.world.robot(0).link( pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1) else: self.firstTimeHack = False robot = motion.robot robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q)) robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q)) qlg = robot.left_gripper.configFromKlampt(q) qrg = robot.right_gripper.configFromKlampt(q) robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg)) robot.right_gripper.command(qrg, [1] * len(qrg), [1] * len(qrg)) #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices]) #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices]) elif c == 'q': motion.robot.shutdown() exit(0) else: GLWidgetProgram.keyboardfunc(self, c, x, y) self.refresh()
def getClosestConfig(self, robot, target, iterations, c, numsteps): """ given a target object position, returns a configuration with end effector close to the target object but without colliding with it""" cost = 9999 res = None start = robot.getConfig() s = IKSolver(robot) objective = IKObjective() objective.setFixedPoint( robot.link("left_gripper:base").getIndex(), (0, 0, 0), target) s.add(objective) s.setActiveDofs([12, 13, 14, 15, 16, 18, 19]) for i in range(iterations): (ret, iters) = s.solve(100, 1e-4) if ret: end = robot.getConfig() qmin, qmax = robot.getJointLimits() flag = False for k in range(len(end)): if end[k] < qmin[k] or end[k] > qmax[k]: flag = True break if flag: start = self.perturb(start, 0.1) robot.setConfig(start) continue for j in xrange(numsteps + 1): u = float(j) / numsteps q = robot.interpolate(end, start, u) if not inCollision(WorldCollider(self.world), robot, q): newcost = vectorops.distance( q, end) + c * vectorops.distance(q, start) if newcost < cost: res = q cost = newcost break start = self.perturb(start, 0.1) robot.setConfig(start) return res
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 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()
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
def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example toggles simulation / movie mode if c == 'h': print '[space]: send the current posed milestone' print 'q: clean quit' elif c == ' ': q = self.robotPoser.get() #print "space q:", q q0 = motion.robot.getKlamptCommandedPosition() #print q0 #collider = WorldCollider(self.world) collisions = obstaclecollision(WorldCollider(self.world), self.world.robot(0), q0, q) for i in range(self.world.robot(0).numLinks()): self.world.robot(0).link(i).appearance().setColor( 0.5, 0.5, 0.5, 1) if not self.firstTimeHack and selfcollision( self.world.robot(0), q0, q): print "Invalid configuration, it self-collides" elif not self.firstTimeHack and collisions != None: #clear all links to gray for pairs in collisions: print "Link " + str( pairs[0].getIndex()) + " collides with obstacle" self.world.robot(0).link( pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1) else: self.firstTimeHack = False robot = motion.robot robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q)) robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q)) print print print "Moving", q0, "->", q print print qlg = robot.left_gripper.configFromKlampt(q) qrg = robot.right_gripper.configFromKlampt(q) print "space prg:", qrg robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg)) robot.right_gripper.command(qrg, [1] * len(qrg), [1] * len(qrg)) #robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices]) #robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices]) elif c == 'q': motion.robot.shutdown() exit(0) elif c == 'p': """given a target object position, automatically moves the end effector close to the target object""" #print "Joint limits:", self.robot.getJointLimits() start = self.world.robot(0).getConfig() #print "end effector original position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0)) #print "start", start """target position currently fixed at (0.8,0.1,1) for testing""" target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1), 100, 0.1, 100) if target == None: # print "Cannot solve IK" return self.robot.setConfig(target) #print "ik result:", target #print "end effector position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0)) path = self.getCollisionFreePath(start, target, 10) #print "path:", path for q in path: robot = motion.robot robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q)) robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q)) qlg = robot.left_gripper.configFromKlampt(q) qrg = robot.right_gripper.configFromKlampt(q) if qlg: robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg)) if qrg: robot.right_gripper.command(qrg, [1] * len(qrg), [1] * len(qrg)) print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition( ) else: GLWidgetProgram.keyboardfunc(self, c, x, y) self.refresh()
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
for i in range(world.numTerrains()): visualization.add("terrain" + str(i), world.terrain(i)) #if you want to just see the robot in a pop up window... if DO_SIMPLIFY and DEBUG_SIMPLIFY: visualization.dialog() #Automatic construction of space if not CLOSED_LOOP_TEST: if not MANUAL_SPACE_CREATION: space = robotplanning.makeSpace(world=world, robot=robot, edgeCheckResolution=1e-2, movingSubset='all') else: #Manual construction of space collider = WorldCollider(world) space = robotcspace.RobotCSpace(robot, collider) space.eps = 1e-2 space.setup() else: #TESTING: closed loop robot cspace collider = WorldCollider(world) obj = ik.objective(robot.link(robot.numLinks() - 1), local=[0, 0, 0], world=[0.5, 0, 0.5]) visualization.add("IK goal", obj) visualization.dialog() space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider) space.eps = 1e-2 space.setup()
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
def __init__(self,world): self.world = world self.robot = world.robot(0) self.collider = WorldCollider(world) self.roadmap = ([],[]) self.dynamic_objects = []
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
def reinit_collider(self): self.collider = WorldCollider(self.world)
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.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 = [] 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
def updateWorld(self,new_world): self.world = new_world self.collider = WorldCollider(new_world)
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 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