Python cspace.CSpace類代碼示例

 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 __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):
     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,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?
     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 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):
     # 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):
     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