def feasible(self, q): #print "checking feasible" isfeasible = not inCollision(self.collider, self.robot, q) and not self.robot.selfCollides() print CSpace.feasible(self, q), q if not CSpace.feasible(self, q): return False return not inCollision(self.collider, self.robot, q) and not self.robot.selfCollides()
def feasible(self,q): if len(q) != len(self.bound): # print len(q), len(self.bound) qnew = q[0:len(self.bound)] q=qnew for i in range(len(q)): # print "q", i, q[i], self.bound[i][0], self.bound[i][1] if (q[i] < self.bound[i][0]) : print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (min:",self.bound[i][0],")" print "Changed joint value to its minimum" q[i] = self.bound[i][0] if (q[i] > self.bound[i][1]) : print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (max:",self.bound[i][1],")" print "Changed joint value to its maximum" q[i] = self.bound[i][1] if not CSpace.feasible(self,q): # print "LimbCSpace.feasible: Configuration is out of bounds" return False # cond = self.planner.check_limb_collision_free(self.limb,q) # if not cond: if not self.planner.check_limb_collision_free(self.limb,q): # print "LimbCSpace.feasible: Configuration is in collision" return False return True
def feasible(self,q): #TODO: implement me #the default simply checks if q is within the bounds return CSpace.feasible(self,q)
def feasible(self,q): if not CSpace.feasible(self,q): # print "LimbCSpace.feasible: Configuration is out of bounds" return False if not self.planner.check_limb_collision_free(self.limb,q): # print "LimbCSpace.feasible: Configuration is in collision" return False return True
def feasible(self, q): #bounds test if not CSpace.feasible(self, q): return False #TODO: Problem 1: implement your feasibility tests here #currently, only the center is checked, so the robot collides #with boundary and obstacles for o in self.obstacles: if o.contains(q): return False return True
def feasible(self,q): #bounds test if not CSpace.feasible(self,q): return False #TODO: Problem 1: implement your feasibility tests here #currently, only the center is checked, so the robot collides #with boundary and obstacles for o in self.obstacles: if o.contains(q): return False return True
def feasible(self,q): if not CSpace.feasible(self,q): #print "LimbCSpace.feasible: Configuration is out of bounds" return False qrob = self.robot.getConfig() self.planner.set_limb_config(self.limb,q,qrob) self.robot.setConfig(qrob) if not self.planner.check_collision_free_with_object(self.limb,self.objectGeom,self.grasp): #print "LimbCSpace.feasible: Configuration is in collision" return False return True
def feasible(self, q): # TODO: implement me # the default simply checks if q is within the bounds # check if the q is within the bounds if CSpace.feasible(self, q): if self.planner.check_limb_collision_free(self.limb, q, True): return True else: return False else: return False
def feasible(self, q): #bounds test if not CSpace.feasible(self, (q[0], q[1], 0)): return False #get the endpoints of the bar p1, p2 = self.endpoints(q) #TODO: implement your feasibility test here: the current implementation #checks endpoints, but doesnt consider collisions with the #intermediate segment between the endpoints for o in self.obstacles: if o.contains(p1) or o.contains(p2): return False return True
def feasible(self,q): #bounds test if not CSpace.feasible(self,(q[0],q[1],0)): return False #get the endpoints of the bar p1,p2 = self.endpoints(q) #TODO: implement your feasibility test here: the current implementation #checks endpoints, but doesnt consider collisions with the #intermediate segment between the endpoints for o in self.obstacles: if o.contains(p1) or o.contains(p2): return False return True
def feasible(self,q): verbose = False#len(self.extra_feasibility_tests)>0 if verbose: print "Custom CSpace test" if not CSpace.feasible(self,q): if verbose: print "Failed CSpace test" return False for index,test in enumerate(self.extra_feasibility_tests): if not test(q): if verbose: print "Extra feasibility test",index,"returned false, returning configuration infeasible" return False if verbose: print "Passed" return True
def feasible(self,q): if not CSpace.feasible(self,q): #print "LimbCSpace.feasible: Configuration is out of bounds" return False if not self.planner.check_limb_collision_free(self.limb,q): #print "LimbCSpace.feasible: Configuration is in collision" return False for i in xrange(self.planner.world.numRigidObjects()): o = self.planner.world.rigidObject(i) g = o.geometry() if g != None and g.type()!="": if self.planner.vacuumPc.withinDistance(g, .03): return False return True
def feasible(self,q): #TODO: implement me #the default simply checks if q is within the bounds #print "Called" #print q, CSpace.feasible(self,q) return CSpace.feasible(self,q) and self.planner.check_limb_collision_free(self.limb, q)
def feasible(self,q): # set robot to given q # get joint 55 orientation # if rotation is not right, return false 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 # self.robot.setConfig(q) # Rcur = self.robot.link(55).getTransform()[0] # # Rgoal # if len(q)>len(self.limb_indices): return False # for i in range(len(q)): # # print "q", i, q[i], self.bound[i][0], self.bound[i][1] # # print self.limb, self.limb_indices, i, len(q) # if (q[i] < self.bound[i][0]) : # print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (min:",self.bound[i][0],")" # print "Changed joint value to its minimum" # q[i] = self.bound[i][0] # if (q[i] > self.bound[i][1]) : # print "Joint #",self.limb_indices[i],"(",q[i],") out of limits (max:",self.bound[i][1],")" # print "Changed joint value to its maximum" # q[i] = self.bound[i][1] # print 1 # # self.robot.setConfig(q) # # return AdaptiveCSpace.feasible(self,q) # print 2 # if not self.inJointLimits(q): return False # print 3 # self.robot.setConfig(q) # if not self.closedLoop(): return False; # #check extras # print 4 # for f in self.extraFeasibilityTesters: # if not f(q): return False # print 5 # #check collisions # if self.selfCollision(): return False # print 6 # if self.envCollision(): return False # return True if not CSpace.feasible(self,q): print "CSpace.feasible: Configuration is out of bounds" return False if not self.planner.check_collision_free(self.limb,q): # print "ClosedLoopRobotCSpace.feasible: Configuration is in collision" return False return True