parameters: (self,Tgrasps,logllthresh=2.0,zaxis=None)
return_statement: return gaussiankerneldensity,gaussiankernelsampler,bounds
docstring: Return a function of the distribution of possible positions of the robot such that any grasp from Tgrasps is reachable.
          Also computes a sampler function that returns a random position of the robot along with the index into Tgrasps
docstring_summary: Return a function of the distribution of possible positions of the robot such that any grasp from Tgrasps is reachable.
          Also computes a sampler function that returns a random position of the robot along with the index into Tgrasps
docstring_tokens: Return a function of the distribution of possible positions of the robot such that any grasp from Tgrasps is reachable . Also computes a sampler function that returns a random position of the robot along with the index into Tgrasps
function: def computeAggregateBaseDistribution(self,Tgrasps,logllthresh=2.0,zaxis=None):
          """Return a function of the distribution of possible positions of the robot such that any grasp from Tgrasps is reachable.
          Also computes a sampler function that returns a random position of the robot along with the index into Tgrasps"""
          if zaxis is not None:
              raise NotImplementedError('cannot specify a custom zaxis yet')
          with self.env:
              Tbase = self.manip.GetBase().GetTransform()
              poserobot = poseFromMatrix(dot(self.robot.GetTransform(),linalg.inv(Tbase)))
          
          rotweight = self.rotweight
          irotweight = 1.0/rotweight
          posebase = poseFromMatrix(Tbase)
          qbaserobotnorm,zbaseangle = normalizeZRotation(reshape(posebase[0:4],(1,4)))
          if quatArrayTDist([1.0,0,0,0],qbaserobotnorm) > 0.05:
              raise planning_error('out of plane rotations for base are not supported')
          
          bandwidth = array((rotweight*self.quatdelta ,self.xyzdelta,self.xyzdelta))
          ibandwidth=-0.5/bandwidth**2
          # normalization for the weights so that integrated volume is 1. this is necessary when comparing across different distributions?
          normalizationconst = (1.0/sqrt(pi**3*sum(bandwidth**2)))
          searchradius=9.0*sum(bandwidth**2)
          searcheps=bandwidth[0]*0.1
          
          points = zeros((0,3))
          weights = array(())
          graspindices = []
          graspindexoffsets = []
          highestlogll = -inf
          for Tgrasp,graspindex in Tgrasps:
              posetarget = poseFromMatrix(dot(linalg.inv(Tbase),Tgrasp))
              qnormalized,znormangle = normalizeZRotation(reshape(posetarget[0:4],(1,4)))
              # find the closest cluster
              logll = quatArrayTDist(qnormalized[0],self.equivalencemeans[:,0:4])**2*self.equivalenceweights[:,0] + (posetarget[6]-self.equivalencemeans[:,4])**2*self.equivalenceweights[:,1] + self.equivalenceoffset
              bestindex = argmax(logll)
              highestlogll = max(highestlogll,logll[bestindex])
              if logll[bestindex] <logllthresh:
                  continue
              graspindices.append(graspindex)
              graspindexoffsets.append(len(points))
              # transform the equivalence class to the global coord system and create a kdtree for faster retrieval
              equivalenceclass = self.equivalenceclasses[bestindex]
              # transform points by the grasp pose
              newpoints = c_[equivalenceclass[2][:,0]+znormangle,dot(equivalenceclass[2][:,1:3],transpose(rotationMatrixFromAxisAngle([0,0,1],znormangle)[0:2,0:2])) + tile(posetarget[4:6], (len(equivalenceclass[2]),1))]
              points = r_[points,newpoints]
              weights = r_[weights,equivalenceclass[2][:,3]*normalizationconst]
  
          if len(points) == 0:
              log.info('inversereachability: could not find base distribution, logllthresh too high? logll=%f', highestlogll)
              return None,None,None
          
          # transform points by the base pose
          points[:,0] += zbaseangle
          points[:,1:3] = dot(points[:,1:3],transpose(rotationMatrixFromAxisAngle([0,0,1],zbaseangle)[0:2,0:2])) + tile(posebase[4:6], (len(points),1))
          
          bounds = array((numpy.min(points,0)-bandwidth,numpy.max(points,0)+bandwidth))
          if bounds[1,0]-bounds[0,0] > 2*pi:
              # already covering entire circle, so limit to 2*pi
              bounds[0,0] = -pi
              bounds[1,0] = pi
          points[:,0] *= rotweight
          kdtree = pyANN.KDTree(points)
          cumweights = cumsum(weights)
          cumweights = cumweights[1:]/cumweights[-1]
          
          def gaussiankerneldensity(poses):
              """returns the density"""
              qposes,zposeangles = normalizeZRotation(poses[:,0:4])
              p = c_[zposeangles*rotweight,poses[:,4:6]]
              neighs,dists,kball = kdtree.kFRSearchArray(p,searchradius,16,searcheps)
              probs = zeros(p.shape[0])
              for i in range(p.shape[0]):
                  inds = neighs[i,neighs[i,:]>=0]
                  if len(inds) > 0:
                      probs[i] = dot(weights[inds],numpy.exp(dot((points[inds,:]-tile(p[i,:],(len(inds),1)))**2,ibandwidth)))
              return probs
          def gaussiankernelsampler(N=1,weight=1.0):
              """samples the distribution and returns a transform as a pose"""
              sampledgraspindices = []
              sampledpoints = zeros((N,3))
              for i in range(N):
                  pointindex = bisect.bisect(cumweights,random.rand())
                  sampledgraspindices.append(graspindices[bisect.bisect(graspindexoffsets,pointindex)-1])
                  sampledpoints[i,:] = points[pointindex,:]
              samples = random.normal(sampledpoints,bandwidth*weight)
              samples[:,0] *= 0.5*irotweight
              return poseMultArrayT(poserobot,c_[cos(samples[:,0]),zeros((N,2)),sin(samples[:,0]),samples[:,1:3],tile(Tbase[2,3],N)]),sampledgraspindices,self.necessaryjointstate()
          return gaussiankerneldensity,gaussiankernelsampler,bounds
function_tokens: def computeAggregateBaseDistribution ( self , Tgrasps , logllthresh = 2.0 , zaxis = None ) : if zaxis is not None : raise NotImplementedError ( 'cannot specify a custom zaxis yet' ) with self . env : Tbase = self . manip . GetBase ( ) . GetTransform ( ) poserobot = poseFromMatrix ( dot ( self . robot . GetTransform ( ) , linalg . inv ( Tbase ) ) ) rotweight = self . rotweight irotweight = 1.0 / rotweight posebase = poseFromMatrix ( Tbase ) qbaserobotnorm , zbaseangle = normalizeZRotation ( reshape ( posebase [ 0 : 4 ] , ( 1 , 4 ) ) ) if quatArrayTDist ( [ 1.0 , 0 , 0 , 0 ] , qbaserobotnorm ) > 0.05 : raise planning_error ( 'out of plane rotations for base are not supported' ) bandwidth = array ( ( rotweight * self . quatdelta , self . xyzdelta , self . xyzdelta ) ) ibandwidth = - 0.5 / bandwidth ** 2 # normalization for the weights so that integrated volume is 1. this is necessary when comparing across different distributions? normalizationconst = ( 1.0 / sqrt ( pi ** 3 * sum ( bandwidth ** 2 ) ) ) searchradius = 9.0 * sum ( bandwidth ** 2 ) searcheps = bandwidth [ 0 ] * 0.1 points = zeros ( ( 0 , 3 ) ) weights = array ( ( ) ) graspindices = [ ] graspindexoffsets = [ ] highestlogll = - inf for Tgrasp , graspindex in Tgrasps : posetarget = poseFromMatrix ( dot ( linalg . inv ( Tbase ) , Tgrasp ) ) qnormalized , znormangle = normalizeZRotation ( reshape ( posetarget [ 0 : 4 ] , ( 1 , 4 ) ) ) # find the closest cluster logll = quatArrayTDist ( qnormalized [ 0 ] , self . equivalencemeans [ : , 0 : 4 ] ) ** 2 * self . equivalenceweights [ : , 0 ] + ( posetarget [ 6 ] - self . equivalencemeans [ : , 4 ] ) ** 2 * self . equivalenceweights [ : , 1 ] + self . equivalenceoffset bestindex = argmax ( logll ) highestlogll = max ( highestlogll , logll [ bestindex ] ) if logll [ bestindex ] < logllthresh : continue graspindices . append ( graspindex ) graspindexoffsets . append ( len ( points ) ) # transform the equivalence class to the global coord system and create a kdtree for faster retrieval equivalenceclass = self . equivalenceclasses [ bestindex ] # transform points by the grasp pose newpoints = c_ [ equivalenceclass [ 2 ] [ : , 0 ] + znormangle , dot ( equivalenceclass [ 2 ] [ : , 1 : 3 ] , transpose ( rotationMatrixFromAxisAngle ( [ 0 , 0 , 1 ] , znormangle ) [ 0 : 2 , 0 : 2 ] ) ) + tile ( posetarget [ 4 : 6 ] , ( len ( equivalenceclass [ 2 ] ) , 1 ) ) ] points = r_ [ points , newpoints ] weights = r_ [ weights , equivalenceclass [ 2 ] [ : , 3 ] * normalizationconst ] if len ( points ) == 0 : log . info ( 'inversereachability: could not find base distribution, logllthresh too high? logll=%f' , highestlogll ) return None , None , None # transform points by the base pose points [ : , 0 ] += zbaseangle points [ : , 1 : 3 ] = dot ( points [ : , 1 : 3 ] , transpose ( rotationMatrixFromAxisAngle ( [ 0 , 0 , 1 ] , zbaseangle ) [ 0 : 2 , 0 : 2 ] ) ) + tile ( posebase [ 4 : 6 ] , ( len ( points ) , 1 ) ) bounds = array ( ( numpy . min ( points , 0 ) - bandwidth , numpy . max ( points , 0 ) + bandwidth ) ) if bounds [ 1 , 0 ] - bounds [ 0 , 0 ] > 2 * pi : # already covering entire circle, so limit to 2*pi bounds [ 0 , 0 ] = - pi bounds [ 1 , 0 ] = pi points [ : , 0 ] *= rotweight kdtree = pyANN . KDTree ( points ) cumweights = cumsum ( weights ) cumweights = cumweights [ 1 : ] / cumweights [ - 1 ] def gaussiankerneldensity ( poses ) : """returns the density""" qposes , zposeangles = normalizeZRotation ( poses [ : , 0 : 4 ] ) p = c_ [ zposeangles * rotweight , poses [ : , 4 : 6 ] ] neighs , dists , kball = kdtree . kFRSearchArray ( p , searchradius , 16 , searcheps ) probs = zeros ( p . shape [ 0 ] ) for i in range ( p . shape [ 0 ] ) : inds = neighs [ i , neighs [ i , : ] >= 0 ] if len ( inds ) > 0 : probs [ i ] = dot ( weights [ inds ] , numpy . exp ( dot ( ( points [ inds , : ] - tile ( p [ i , : ] , ( len ( inds ) , 1 ) ) ) ** 2 , ibandwidth ) ) ) return probs def gaussiankernelsampler ( N = 1 , weight = 1.0 ) : """samples the distribution and returns a transform as a pose""" sampledgraspindices = [ ] sampledpoints = zeros ( ( N , 3 ) ) for i in range ( N ) : pointindex = bisect . bisect ( cumweights , random . rand ( ) ) sampledgraspindices . append ( graspindices [ bisect . bisect ( graspindexoffsets , pointindex ) - 1 ] ) sampledpoints [ i , : ] = points [ pointindex , : ] samples = random . normal ( sampledpoints , bandwidth * weight ) samples [ : , 0 ] *= 0.5 * irotweight return poseMultArrayT ( poserobot , c_ [ cos ( samples [ : , 0 ] ) , zeros ( ( N , 2 ) ) , sin ( samples [ : , 0 ] ) , samples [ : , 1 : 3 ] , tile ( Tbase [ 2 , 3 ] , N ) ] ) , sampledgraspindices , self . necessaryjointstate ( ) return gaussiankerneldensity , gaussiankernelsampler , bounds