def __init__(self,planner,limb): CSpace.__init__(self) self.planner = planner self.limb = limb #TODO: what Cartesian bounding box should the planner sample from? self.bound = [(0,1),(0,1)] #TODO: what collision tolerance to use? self.eps = 1e-2
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 __init__(self,planner,limb): CSpace.__init__(self) self.planner = planner self.limb = limb # set up a search space based on the robot 1 m x 2 m x 2 m (R, t) = self.planner.robot.getLink(0).getParentTransform() self.bound = self.bounds = [(t[0]-1, t[0]+2), (t[1]-2, t[2]+2), (0, 3)] self.eps = 1e-2 self.ignore = None
def __init__(self, collider, robot): CSpace.__init__(self) qmin, qmax = robot.getJointLimits() self.bound = [] for i in range(len(qmin)): self.bound.append((qmin[i], qmax[i])) self.eps = 1e-3 self.collider = collider self.robot = robot
def __init__(self): CSpace.__init__(self) #set bounds self.bound = [(0.0, 1.0), (0.0, 1.0)] #set collision checking resolution self.eps = 1e-3 #setup a robot with radius 0.05 self.robot = Circle(0, 0, 0.05) #set obstacles here self.obstacles = []
def __init__(self): CSpace.__init__(self) #set bounds self.bound = [(0.0, 1.0), (0.0, 1.0), (0.0, math.pi * 2)] #set collision checking resolution self.eps = 1e-3 #setup a bar with length 0.1 self.L = 0.1 #set obstacles here self.obstacles = []
def __init__(self): CSpace.__init__(self) #set bounds self.bound = [(0.0,1.0),(0.0,1.0),(0.0,math.pi*2)] #set collision checking resolution self.eps = 1e-3 #setup a bar with length 0.1 self.L = 0.1 #set obstacles here self.obstacles = []
def __init__(self): CSpace.__init__(self) #set bounds self.bound = [(0.0,1.0),(0.0,1.0)] #set collision checking resolution self.eps = 1e-3 #setup a robot with radius 0.05 self.robot = Circle(0,0,0.05) #set obstacles here self.obstacles = []
def __init__(self,planner,limb): CSpace.__init__(self) self.planner = planner ''' :type self.planner: LimbPlanner ''' self.limb = limb #TODO: what Cartesian bounding box should the planner sample from? self.bound = [(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi),(-pi,pi)] #TODO: what collision tolerance to use? self.eps = 1e-2
def __init__(self,planner): CSpace.__init__(self) self.planner = planner #TODO: what Cartesian bounding box should the planner sample from? self.robot = self.planner.robot id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())]) qmin,qmax = self.robot.getJointLimits() self.bound = [(qmin[i],qmax[i]) for i in range(self.robot.numLinks())] self.eps = 1e-3
def __init__(self,globals,hand): CSpace.__init__(self) self.globals = globals self.robot = globals.robot self.hand = hand #initial whole-body configuratoin self.q0 = self.robot.getConfig() #setup CSpace sampling range qlimits = zip(*self.robot.getJointLimits()) self.bound = [qlimits[i] for i in self.hand.armIndices] #setup CSpace edge checking epsilon self.eps = 1e-2
def __init__(self, globals, hand): CSpace.__init__(self) self.globals = globals self.robot = globals.robot self.hand = hand #initial whole-body configuratoin self.q0 = self.robot.getConfig() #setup CSpace sampling range qlimits = zip(*self.robot.getJointLimits()) self.bound = [qlimits[i] for i in self.hand.armIndices] #setup CSpace edge checking epsilon self.eps = 1e-2
def __init__(self,planner,limb): CSpace.__init__(self) self.planner = planner self.limb = limb #TODO: what Cartesian bounding box should the planner sample from? self.robot = self.planner.robot id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())]) if limb=='left': self.limb_indices = [id_to_index[i.getID()] for i in planner.left_arm_links] else: self.limb_indices = [id_to_index[i.getID()] for i in planner.right_arm_links] qmin,qmax = self.robot.getJointLimits() self.bound = [(qmin[i]-1e-6,qmax[i]+1e-6) for i in self.limb_indices] self.eps = 1e-1
def __init__(self,planner,limb): CSpace.__init__(self) self.planner = planner self.limb = limb #TODO: what Cartesian bounding box should the planner sample from? get_limb_config(planner.robot.getConfig(), self.limb) self.setup() self.milestone.getPath self.bound = [(0,1),(0,1)] self.setBounds(self.bound) #TODO: what collision tolerance to use? self.eps = 1e-2
def feasible(self,q): #TODO: implement me #the default simply checks if q is within the bounds return CSpace.feasible(self,q)
def __init__(self,planner,limb,obj,grasp): CSpace.__init__(self) self.planner = planner self.limb = limb self.object = obj self.grasp = grasp self.objectGeom = obj.info.geometry self.robot = self.planner.robot id_to_index = dict([(self.robot.link(i).getID(),i) for i in range(self.robot.numLinks())]) if limb=='left': self.limb_indices = [id_to_index[i.getID()] for i in planner.left_arm_links] else: self.limb_indices = [id_to_index[i.getID()] for i in planner.right_arm_links] qmin,qmax = self.robot.getJointLimits() self.bound = [(qmin[i]-1e-6,qmax[i]+1e-6) for i in self.limb_indices] self.eps = 1e-1
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 __init__(self, planner, limb): CSpace.__init__(self) self.planner = planner self.limb = limb # TODO: what Cartesian bounding box should the planner sample from? self.robot = self.planner.robot id_to_index = dict([(self.robot.link(i).getID(), i) for i in range(self.robot.numLinks())]) if limb == "left": # self.limb_indices = left_arm_geometry_indices + left_hand_geometry_indices self.limb_indices = self.planner.left_arm_indices else: # self.limb_indices = right_arm_geometry_indices + right_hand_geometry_indices self.limb_indices = self.planner.right_arm_indices qmin, qmax = self.robot.getJointLimits() self.bound = [(qmin[i] - 1e-6, qmax[i] + 1e-6) for i in self.limb_indices] self.eps = 1e-1
def __init__(self, globals, hand, object): CSpace.__init__(self) self.globals = globals self.robot = globals.robot self.hand = hand self.object = object #setup initial object-robot transform Thand0 = self.robot.link(hand.link).getTransform() Tobj0 = object.getTransform() self.Tgrasp = se3.mul(se3.inv(Thand0), Tobj0) #initial whole-body configuratoin self.q0 = self.robot.getConfig() #setup CSpace sampling range qlimits = zip(*self.robot.getJointLimits()) self.bound = [qlimits[i] for i in self.hand.armIndices] #setup CSpace edge checking epsilon self.eps = 1e-2
def __init__(self,globals,hand,object): CSpace.__init__(self) self.globals = globals self.robot = globals.robot self.hand = hand self.object = object #setup initial object-robot transform Thand0 = self.robot.link(hand.link).getTransform() Tobj0 = object.getTransform() self.Tgrasp = se3.mul(se3.inv(Thand0),Tobj0) #initial whole-body configuratoin self.q0 = self.robot.getConfig() #setup CSpace sampling range qlimits = zip(*self.robot.getJointLimits()) self.bound = [qlimits[i] for i in self.hand.armIndices] #setup CSpace edge checking epsilon self.eps = 1e-2
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): # 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): 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): #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): #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): 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): # 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
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 __init__(self): CSpace.__init__(self) self.extra_feasibility_tests = []