def __init__(self,spaces): CSpace.__init__(self) self.spaces = spaces #construct optional methods def sampleneighborhood(self,c,r): return self.join(s.sampleNeighborhood(cs,r) for (s,cs) in zip(self.spaces,self.split(c))) def visible(self,a,b): return all(s.visible(ai,bi) for (s,ai,bi) in zip(self.spaces,self.split(a),self.split(b))) def distance(self,a,b): return sum(s.distance(ai,bi) for (s,ai,bi) in zip(self.spaces,self.split(a),self.split(b))) def interpolate(self,a,b,u): return self.join(s.interpolate(ai,bi,u) for (s,ai,bi) in zip(self.spaces,self.split(a),self.split(b))) if any(hasattr(s,'sampleneighborhood') for s in spaces): for s in self.spaces: if not hasattr(s,'sampleneighborhood'): s.sampleneighborhood = defaultsampleneighborhood self.sampleneighborhood = sampleneighborhood if any(hasattr(s,'visible') for s in spaces): for s in self.spaces: if not hasattr(s,'visible'): s.visible = defaultvisible self.visible = visible if any(hasattr(s,'distance') for s in spaces): for s in self.spaces: if not hasattr(s,'distance'): s.distance = defaultdistance self.distance = distance if any(hasattr(s,'interpolate') for s in spaces): for s in self.spaces: if not hasattr(s,'interpolate'): s.interpolate = defaultinterpolate self.interpolate = interpolate
def __init__(self,robot,collider=None): """Arguments: - robot: the robot which should move. - collider: optional: a robotcollide.WorldCollider instance containing the world in which the robot lives. Any ignored collisions will be respected in the collision checker. """ CSpace.__init__(self) self.robot = robot self.bound = zip(*robot.getJointLimits()) self.collider = collider self.addFeasibilityTest(lambda x: self.inJointLimits(x),"joint limits") #TODO explode these into individual self collision / env collision #tests def setconfig(x): self.robot.setConfig(x) return True if collider: self.addFeasibilityTest(setconfig,"dummy") self.addFeasibilityTest(lambda x : not self.selfCollision(),"self collision") self.addFeasibilityTest(lambda x: not self.envCollision(),"env collision") self.properties['geodesic'] = 1 volume = 1 for b in self.bound: if b[0] != b[1]: volume *= b[1]-b[0] self.properties['volume'] = volume
def __init__(self, robot, collider=None): """Arguments: - robot: the robot which should move. - collider: optional: a robotcollide.WorldCollider instance containing the world in which the robot lives. Any ignored collisions will be respected in the collision checker. """ CSpace.__init__(self) self.robot = robot self.bound = zip(*robot.getJointLimits()) self.collider = collider self.addFeasibilityTest(lambda x: self.inJointLimits(x), "joint limits") #TODO explode these into individual self collision / env collision #tests def setconfig(x): self.robot.setConfig(x) return True if collider: self.addFeasibilityTest(setconfig, "dummy") self.addFeasibilityTest(lambda x: not self.selfCollision(), "self collision") self.addFeasibilityTest(lambda x: not self.envCollision(), "env collision") self.properties['geodesic'] = 1 volume = 1 for b in self.bound: if b[0] != b[1]: volume *= b[1] - b[0] self.properties['volume'] = volume
def __init__(self,ambientspace,subset,xinit=None): CSpace.__init__(self) self.ambientspace = ambientspace n = len(ambientspace.sample()) self.mapping = subset #start at the zero config if no initial configuration is given if xinit==None: self.xinit = [0.0]*n else: self.xinit = xinit #construct optional methods def sampleneighborhood(c,r): return self.project(self.ambientspace.sampleneighborhood(self.lift(c),r)) def visible(a,b): return self.ambientspace.visible(self.lift(a),self.lift(b)) def distance(a,b): return self.ambientspace.distance(self.lift(a),self.lift(b)) def interpolate(a,b,u): return self.project(self.ambientspace.interpolate(self.lift(a)),self.lift(b)) if hasattr(ambientspace,'sampleneighborhood'): self.sampleneighborhood = sampleneighborhood if hasattr(ambientspace,'visible'): self.visible = visible if hasattr(ambientspace,'distance'): self.distance = distance if hasattr(ambientspace,'interpolate'): self.interpolate = interpolate self.eps = self.ambientspace.eps self.bound = [self.ambientspace.bound[i] for i in self.mapping] self.properties = self.ambientspace.properties if self.ambientspace.feasibilityTests is not None: self.feasibilityTests = [(lambda x:f(self.lift(x))) for f in self.ambientspace.feasibilityTests] self.feasibilityTestNames = [(lambda x:f(self.lift(x))) for f in self.ambientspace.feasibilityTestNames]
def __init__(self,rigidObject,collider=None,translationDomain=None,rotationDomain=None): """ Args: rigidObject (RigidObjectModel): the object that should move. collider (:class:`WorldCollider`, optional): a collider instance containing the world in which the object lives. Any ignored collisions will be respected in the feasibility test. translationDomain (list of pairs, optional): a bounding box in which the translation should be sampled. If None (default), the improper logarithmic prior is used to sample translations. rotationDomain (pair, optional): If provided, must be a (rotation0,rdomain) pair specifying a range in which the rotation should be sampled. rotation0 must be an SO3 element. rdomain may be: * A number: rotation is sampled with absolute angular error from rotation0 in the range [0,rdomain]. * A triple: rotation is sampled with euler angles with roll in the range [-rdomain[0],rdomain[0]], pitch in the range [-rdomain[1],rdomain[1]], and yaw in the range [-rdomain[2],rdomain[2]]. The sampled rotation is then multiplied by rotation0. """ CSpace.__init__(self) self.rigidObject = rigidObject if translationDomain is None: translationDomain = [(-float('inf'),float('inf'))]*3 self.bound = translationDomain + [(-math.pi,math.pi)]*3 self.rotationDomain = rotationDomain self.collider = collider self.rotationWeight = 1.0/math.pi if collider: def robCollide(r): return any(True for _ in self.collider.robotObjectCollisions(r,self.rigidObject.index)) def objCollide(o): return any(True for _ in self.collider.objectObjectCollisions(self.rigidObject.index,o)) def terrCollide(o): return any(True for _ in self.collider.objectTerrainCollisions(self.rigidObject.index,o)) self.addFeasibilityTest(self.setConfig,"setconfig") self.addFeasibilityTest((lambda x: not self.selfCollision()),"self collision",dependencies="setconfig") #self.addFeasibilityTest((lambda x: not self.envCollision()),"env collision") for o in range(self.collider.world.numRobots()): self.addFeasibilityTest((lambda x,o=o: not robCollide(o)),"robot collision "+str(o)+" "+self.collider.world.robot(o).getName(),dependencies="setconfig") for o in range(self.collider.world.numRigidObjects()): if o != self.rigidObject: self.addFeasibilityTest((lambda x,o=o: not objCollide(o)),"obj collision "+str(o)+" "+self.collider.world.rigidObject(o).getName(),dependencies="setconfig") for o in range(self.collider.world.numTerrains()): self.addFeasibilityTest((lambda x,o=o: not terrCollide(o)),"terrain collision "+str(o)+" "+self.collider.world.terrain(o).getName(),dependencies="setconfig") else: self.addFeasibilityTest(self.setConfig,"setconfig") self.properties['geodesic'] = 1
def __init__(self,robot,collider=None): """ Args: robot (RobotModel): the robot which should move. collider (:class:`WorldCollider`, optional): a collide.WorldCollider instance instantiated with the world in which the robot lives. Any ignored collisions in the collider will be respected in the feasibility tests of this CSpace. """ CSpace.__init__(self) self.robot = robot self.setBounds(zip(*robot.getJointLimits())) self.collider = collider self.addFeasibilityTest((lambda x: self.inJointLimits(x)),"joint limits") def setconfig(x): self.robot.setConfig(x) return True if collider: bb0 = ([float('inf')]*3,[float('-inf')]*3) bb = [bb0[0],bb0[1]] def calcbb(x): bb[0] = bb0[0] bb[1] = bb0[1] for i in xrange(self.robot.numLinks()): g = self.robot.link(i).geometry() if not g.empty(): bbi = g.getBB() bb[0] = [min(a,b) for (a,b) in zip(bb[0],bbi[0])] bb[1] = [max(a,b) for (a,b) in zip(bb[1],bbi[1])] return True def objCollide(o): obb = self.collider.world.rigidObject(o).geometry().getBB() if not collide.bb_intersect(obb,bb): return False return any(True for _ in self.collider.robotObjectCollisions(self.robot.index,o)) def terrCollide(o): obb = self.collider.world.terrain(o).geometry().getBB() if not collide.bb_intersect(obb,bb): return False return any(True for _ in self.collider.robotTerrainCollisions(self.robot.index,o)) self.addFeasibilityTest(setconfig,"setconfig") self.addFeasibilityTest(calcbb,"calcbb",dependencies="setconfig") self.addFeasibilityTest((lambda x: not self.selfCollision()),"self collision",dependencies="setconfig") #self.addFeasibilityTest((lambda x: not self.envCollision()),"env collision") for o in range(self.collider.world.numRigidObjects()): self.addFeasibilityTest((lambda x,o=o: not objCollide(o)),"obj collision "+str(o)+" "+self.collider.world.rigidObject(o).getName(),dependencies="calcbb") for o in range(self.collider.world.numTerrains()): self.addFeasibilityTest((lambda x,o=o: not terrCollide(o)),"terrain collision "+str(o)+" "+self.collider.world.terrain(o).getName(),dependencies="calcbb") else: self.addFeasibilityTest(setconfig,"setconfig") self.addFeasibilityTest((lambda x: not self.selfCollision()),"self collision",dependencies="setconfig") self.properties['geodesic'] = 1
def __init__(self, spaces): CSpace.__init__(self) self.spaces = spaces #construct optional methods def sampleneighborhood(self, c, r): return self.join( s.sampleNeighborhood(cs, r) for (s, cs) in zip(self.spaces, self.split(c))) def visible(self, a, b): return all( s.visible(ai, bi) for (s, ai, bi) in zip(self.spaces, self.split(a), self.split(b))) def distance(self, a, b): return sum( s.distance(ai, bi) for (s, ai, bi) in zip(self.spaces, self.split(a), self.split(b))) def interpolate(self, a, b, u): return self.join( s.interpolate(ai, bi, u) for (s, ai, bi) in zip(self.spaces, self.split(a), self.split(b))) if any(hasattr(s, 'sampleneighborhood') for s in spaces): for s in self.spaces: if not hasattr(s, 'sampleneighborhood'): s.sampleneighborhood = defaultsampleneighborhood self.sampleneighborhood = sampleneighborhood if any(hasattr(s, 'visible') for s in spaces): for s in self.spaces: if not hasattr(s, 'visible'): s.visible = defaultvisible self.visible = visible if any(hasattr(s, 'distance') for s in spaces): for s in self.spaces: if not hasattr(s, 'distance'): s.distance = defaultdistance self.distance = distance if any(hasattr(s, 'interpolate') for s in spaces): for s in self.spaces: if not hasattr(s, 'interpolate'): s.interpolate = defaultinterpolate self.interpolate = interpolate
def __init__(self, ambientspace, subset, xinit=None): CSpace.__init__(self) self.ambientspace = ambientspace n = len(ambientspace.sample()) self.mapping = subset #start at the zero config if no initial configuration is given if xinit == None: self.xinit = [0.0] * n else: self.xinit = xinit #construct optional methods def sampleneighborhood(c, r): return self.project( self.ambientspace.sampleneighborhood(self.lift(c), r)) def visible(a, b): return self.ambientspace.visible(self.lift(a), self.lift(b)) def distance(a, b): return self.ambientspace.distance(self.lift(a), self.lift(b)) def interpolate(a, b, u): return self.project(self.ambientspace.interpolate(self.lift(a)), self.lift(b)) if hasattr(ambientspace, 'sampleneighborhood'): self.sampleneighborhood = sampleneighborhood if hasattr(ambientspace, 'visible'): self.visible = visible if hasattr(ambientspace, 'distance'): self.distance = distance if hasattr(ambientspace, 'interpolate'): self.interpolate = interpolate self.eps = self.ambientspace.eps self.bound = [self.ambientspace.bound[i] for i in self.mapping] self.properties = self.ambientspace.properties if self.ambientspace.feasibilityTests is not None: self.feasibilityTests = [ (lambda x: f(self.lift(x))) for f in self.ambientspace.feasibilityTests ] self.feasibilityTestNames = [ (lambda x: f(self.lift(x))) for f in self.ambientspace.feasibilityTestNames ]
def __init__(self, rigidObject, collider=None, translationDomain=None, rotationDomain=None): """ Args: rigidObject (RigidObjectModel): the object that should move. collider (:class:`WorldCollider`, optional): a collider instance containing the world in which the object lives. Any ignored collisions will be respected in the feasibility test. translationDomain (list of pairs, optional): a bounding box in which the translation should be sampled. If None (default), the improper logarithmic prior is used to sample translations. rotationDomain (pair, optional): If provided, must be a (rotation0,rdomain) pair specifying a range in which the rotation should be sampled. rotation0 must be an SO3 element. rdomain may be: * A number: rotation is sampled with absolute angular error from rotation0 in the range [0,rdomain]. * A triple: rotation is sampled with euler angles with roll in the range [-rdomain[0],rdomain[0]], pitch in the range [-rdomain[1],rdomain[1]], and yaw in the range [-rdomain[2],rdomain[2]]. The sampled rotation is then multiplied by rotation0. """ CSpace.__init__(self) self.rigidObject = rigidObject if translationDomain is None: translationDomain = [(-float('inf'), float('inf'))] * 3 self.bound = translationDomain + [(-math.pi, math.pi)] * 3 self.rotationDomain = rotationDomain self.collider = collider self.rotationWeight = 1.0 / math.pi if collider: def robCollide(r): return any(True for _ in self.collider.robotObjectCollisions( r, self.rigidObject.index)) def objCollide(o): return any(True for _ in self.collider.objectObjectCollisions( self.rigidObject.index, o)) def terrCollide(o): return any(True for _ in self.collider.objectTerrainCollisions( self.rigidObject.index, o)) self.addFeasibilityTest(self.setConfig, "setconfig") for o in range(self.collider.world.numRobots()): self.addFeasibilityTest((lambda x, o=o: not robCollide(o)), "robot collision " + str(o) + " " + self.collider.world.robot(o).getName(), dependencies="setconfig") for o in range(self.collider.world.numRigidObjects()): if o != self.rigidObject.index: self.addFeasibilityTest( (lambda x, o=o: not objCollide(o)), "obj collision " + str(o) + " " + self.collider.world.rigidObject(o).getName(), dependencies="setconfig") for o in range(self.collider.world.numTerrains()): self.addFeasibilityTest( (lambda x, o=o: not terrCollide(o)), "terrain collision " + str(o) + " " + self.collider.world.terrain(o).getName(), dependencies="setconfig") else: self.addFeasibilityTest(self.setConfig, "setconfig") self.properties['geodesic'] = 1
def __init__(self): CSpace.__init__(self) AdaptiveZeroTester.__init__(self)
def __init__(self, rigidObject, collider=None, translationDomain=None, rotationDomain=None): """Arguments: - rigidObject: the RigidObjectModel that should move. - collider (optional): a collide.WorldCollider instance containing the world in which the robot lives. Any ignored collisions will be respected in the collision checker. - translationDomain: None, or a bounding box in which the translation should be sampled. If None (default), the Jeffrey's prior is used to sample translations. - rotationDomain: None, or a (rotation0,rdomain) pair specifying a range in which the rotation should be sampled. If rdomain is a number, the rotation is sampled with absolute angular error from rotation0 in the range [0,rdomain]. If rdomain is a triple, then the rotation is sampled with euler angles with roll in the range [-rdomain[0],rdomain[0]], pitch in the range [-rdomain[1],rdomain[1]], and yaw in the range [-rdomain[2],rdomain[2]]. The sampled rotation is then multiplied by rotation0. """ CSpace.__init__(self) self.rigidObject = rigidObject if translationDomain is None: translationDomain = [(-float('inf'), float('inf'))] * 3 self.bound = translationDomain + [(-math.pi, math.pi)] * 3 self.rotationDomain = rotationDomain self.collider = collider self.rotationWeight = 1.0 / math.pi if collider: def robCollide(r): return any(True for _ in self.collider.robotObjectCollisions( r, self.rigidObject.index)) def objCollide(o): return any(True for _ in self.collider.objectObjectCollisions( self.rigidObject.index, o)) def terrCollide(o): return any(True for _ in self.collider.objectTerrainCollisions( self.rigidObject.index, o)) self.addFeasibilityTest(setconfig, "setconfig") self.addFeasibilityTest((lambda x: not self.selfCollision()), "self collision", dependencies="setconfig") #self.addFeasibilityTest((lambda x: not self.envCollision()),"env collision") for o in range(self.collider.world.numRobots()): self.addFeasibilityTest((lambda x, o=o: not robCollide(o)), "robot collision " + str(o) + " " + self.collider.world.robot(o).getName(), dependencies="setconfig") for o in range(self.collider.world.numRigidObjects()): if o != self.rigidObject: self.addFeasibilityTest( (lambda x, o=o: not objCollide(o)), "obj collision " + str(o) + " " + self.collider.world.rigidObject(o).getName(), dependencies="setconfig") for o in range(self.collider.world.numTerrains()): self.addFeasibilityTest( (lambda x, o=o: not terrCollide(o)), "terrain collision " + str(o) + " " + self.collider.world.terrain(o).getName(), dependencies="setconfig") else: self.addFeasibilityTest(self.setConfig, "setconfig") self.properties['geodesic'] = 1