parameters: (link : RobotModelLink,
                              constraint_or_point : Union[Vector3,IKObjective],
                              value : Union[str,Callable],
                              Nsamples=100000,
                              resolution=0.05,
                              dimensions=None,
                              fixed_links : Sequence[Union[int,str]] = None,
                              qmin : Config = None,
                              qmax : Config = None,
                              obstacles : Sequence[Union[RigidObjectModel,TerrainModel]] = None,
                              load : Union[float,Vector3] = None,
                              load_type : str = None,
                              gravity : Vector3 = (0,0,-9.8),
                              self_collision : bool = True,
                              feasibility_test : Callable = None)
return_statement: return res
docstring: Compute a scalar field over the reachable workspace of a point on a 
      robot's end effector. Has several options to ensure various feasibility 
      conditions. Ensures that the robot does not collide with itself, optionally
      with obstacles.
      
      Arguments:
          link (RobotModelLink): the link on the robot for which the workspace
              should be computed
          constraint_or_point (3-vector or IKObjective): the point on the link.
              For fixed orientations, use an IKObjective.
          value (str or callable): 'occupancy', 'manipulability',
              'max manipulability', 'min manipulability', 'load', or an arbitrary
              function f(q) -> float.
          Nsamples (int): the number of configuration samples to use
          resolution (float, 3-vector, or None): the resolution of the resulting
              grid.
          dimensions (int or 3-vector optional): if resolution=None and
              dimensions is given, the number of dimensions. If a single int, the
              cell size is determined by dividing the longest side by dimensions.
          fixed_links (list of int or str, optional): list of fixed link indices
          qmin (Config, optional): overrides the robots lower joint limits
          qmax (Config, optional): overrides the robots upper joint limits
          obstacles (list of RigidObjectModel or TerrainModel): Ensures that the
              robot is collision free with environment obstacles
          load (float or vector, optional): a required load capacity, either an
              isotropic load or a specific vector
          load_type (str, optional): either 'auto' (default), 'isotropic',
              'force', 'torque', 'wrench', 'force_local', 'torque_local', or
              'wrench_local'.
          gravity (3-vector, optional): if load testing is used, the gravity
              vector used for gravity compensation.
          self_collision (bool): whether self collision should be tested.
          feasibility_test (callable, list, or dict): a function
              feasible(q)->bool that tests whether the configuration is feasible.
              If a list, a list of callables. If a dict, a map of str->callable.
  
      Returns:
          The grid of reached points.
          If ``all_tests=True``, gives a set of grids of reached points.  Key
          'workspace' gives the reachable workspace of points meeting every
          feasibility condition.  Keys 'self_collision_free',
          'obstacle_X_collision_free' (X in range 0,...,len(obstacles)-1),
          'load', 'feasibility_test' give the set of points passing each
          feasibility condition.
docstring_summary: Compute a scalar field over the reachable workspace of a point on a 
      robot's end effector. Has several options to ensure various feasibility 
      conditions. Ensures that the robot does not collide with itself, optionally
      with obstacles.
      
      Arguments:
          link (RobotModelLink): the link on the robot for which the workspace
              should be computed
          constraint_or_point (3-vector or IKObjective): the point on the link.
              For fixed orientations, use an IKObjective.
          value (str or callable): 'occupancy', 'manipulability',
              'max manipulability', 'min manipulability', 'load', or an arbitrary
              function f(q) -> float.
          Nsamples (int): the number of configuration samples to use
          resolution (float, 3-vector, or None): the resolution of the resulting
              grid.
          dimensions (int or 3-vector optional): if resolution=None and
              dimensions is given, the number of dimensions. If a single int, the
              cell size is determined by dividing the longest side by dimensions.
          fixed_links (list of int or str, optional): list of fixed link indices
          qmin (Config, optional): overrides the robots lower joint limits
          qmax (Config, optional): overrides the robots upper joint limits
          obstacles (list of RigidObjectModel or TerrainModel): Ensures that the
              robot is collision free with environment obstacles
          load (float or vector, optional): a required load capacity, either an
              isotropic load or a specific vector
          load_type (str, optional): either 'auto' (default), 'isotropic',
              'force', 'torque', 'wrench', 'force_local', 'torque_local', or
              'wrench_local'.
          gravity (3-vector, optional): if load testing is used, the gravity
              vector used for gravity compensation.
          self_collision (bool): whether self collision should be tested.
          feasibility_test (callable, list, or dict): a function
              feasible(q)->bool that tests whether the configuration is feasible.
              If a list, a list of callables. If a dict, a map of str->callable.
docstring_tokens: Compute a scalar field over the reachable workspace of a point on a robot s end effector . Has several options to ensure various feasibility conditions . Ensures that the robot does not collide with itself optionally with obstacles . Arguments : link ( RobotModelLink ) : the link on the robot for which the workspace should be computed constraint_or_point ( 3 - vector or IKObjective ) : the point on the link . For fixed orientations use an IKObjective . value ( str or callable ) : occupancy manipulability max manipulability min manipulability load or an arbitrary function f ( q ) - > float . Nsamples ( int ) : the number of configuration samples to use resolution ( float 3 - vector or None ) : the resolution of the resulting grid . dimensions ( int or 3 - vector optional ) : if resolution = None and dimensions is given the number of dimensions . If a single int the cell size is determined by dividing the longest side by dimensions . fixed_links ( list of int or str optional ) : list of fixed link indices qmin ( Config optional ) : overrides the robots lower joint limits qmax ( Config optional ) : overrides the robots upper joint limits obstacles ( list of RigidObjectModel or TerrainModel ) : Ensures that the robot is collision free with environment obstacles load ( float or vector optional ) : a required load capacity either an isotropic load or a specific vector load_type ( str optional ) : either auto ( default ) isotropic force torque wrench force_local torque_local or wrench_local . gravity ( 3 - vector optional ) : if load testing is used the gravity vector used for gravity compensation . self_collision ( bool ) : whether self collision should be tested . feasibility_test ( callable list or dict ) : a function feasible ( q ) - > bool that tests whether the configuration is feasible . If a list a list of callables . If a dict a map of str - > callable .
function: def compute_workspace_field(link : RobotModelLink,
                              constraint_or_point : Union[Vector3,IKObjective],
                              value : Union[str,Callable],
                              Nsamples=100000,
                              resolution=0.05,
                              dimensions=None,
                              fixed_links : Sequence[Union[int,str]] = None,
                              qmin : Config = None,
                              qmax : Config = None,
                              obstacles : Sequence[Union[RigidObjectModel,TerrainModel]] = None,
                              load : Union[float,Vector3] = None,
                              load_type : str = None,
                              gravity : Vector3 = (0,0,-9.8),
                              self_collision : bool = True,
                              feasibility_test : Callable = None) -> VolumeGrid:
      """Compute a scalar field over the reachable workspace of a point on a 
      robot's end effector. Has several options to ensure various feasibility 
      conditions. Ensures that the robot does not collide with itself, optionally
      with obstacles.
      
      Arguments:
          link (RobotModelLink): the link on the robot for which the workspace
              should be computed
          constraint_or_point (3-vector or IKObjective): the point on the link.
              For fixed orientations, use an IKObjective.
          value (str or callable): 'occupancy', 'manipulability',
              'max manipulability', 'min manipulability', 'load', or an arbitrary
              function f(q) -> float.
          Nsamples (int): the number of configuration samples to use
          resolution (float, 3-vector, or None): the resolution of the resulting
              grid.
          dimensions (int or 3-vector optional): if resolution=None and
              dimensions is given, the number of dimensions. If a single int, the
              cell size is determined by dividing the longest side by dimensions.
          fixed_links (list of int or str, optional): list of fixed link indices
          qmin (Config, optional): overrides the robots lower joint limits
          qmax (Config, optional): overrides the robots upper joint limits
          obstacles (list of RigidObjectModel or TerrainModel): Ensures that the
              robot is collision free with environment obstacles
          load (float or vector, optional): a required load capacity, either an
              isotropic load or a specific vector
          load_type (str, optional): either 'auto' (default), 'isotropic',
              'force', 'torque', 'wrench', 'force_local', 'torque_local', or
              'wrench_local'.
          gravity (3-vector, optional): if load testing is used, the gravity
              vector used for gravity compensation.
          self_collision (bool): whether self collision should be tested.
          feasibility_test (callable, list, or dict): a function
              feasible(q)->bool that tests whether the configuration is feasible.
              If a list, a list of callables. If a dict, a map of str->callable.
  
      Returns:
          The grid of reached points.
          If ``all_tests=True``, gives a set of grids of reached points.  Key
          'workspace' gives the reachable workspace of points meeting every
          feasibility condition.  Keys 'self_collision_free',
          'obstacle_X_collision_free' (X in range 0,...,len(obstacles)-1),
          'load', 'feasibility_test' give the set of points passing each
          feasibility condition.
      """
      robot = link.robot()
      qmin_orig,qmax_orig = robot.getJointLimits()
      
      if qmin is None:
          qmin = qmin_orig
      else:
          if len(qmin)!=len(qmin_orig):
              raise ValueError("Invalid length of qmin")
      if qmax is None:
          qmax = qmax_orig
      else:
          if len(qmax)!=len(qmax_orig):
              raise ValueError("Invalid length of qmin")
      if fixed_links is not None:
          import copy
          q = robot.getConfig()
          qmin = copy.copy(qmin)
          qmax = copy.copy(qmax)
          for l in fixed_links:
              i = robot.link(l).getIndex()
              qmin[i] = q[i]
              qmax[i] = q[i]
          robot.setJointLimits(qmin,qmax)
      else:
          robot.setJointLimits(qmin,qmax)
  
      if isinstance(constraint_or_point,IKObjective):
          point_local,_ = constraint_or_point.getPosition()
          constraint = constraint_or_point
      else:
          point_local = constraint_or_point
          constraint = None
  
      #setup feasibility test
      feasibleTests = _setup_feasible_tests(robot,link,point_local,fixed_links,
          obstacles,load,load_type,gravity,self_collision,feasibility_test)
      def overall_feasibility_test():
          return all(test() for k,test in feasibleTests.items())
  
      if isinstance(value,callable):
          value_fn = lambda : value(robot.getConfig())
      elif value == 'occupancy':
          value_fn = lambda : 1
      elif value == 'load':
          assert 'load' in feasibleTests
          raise NotImplementedError("TODO: do load calculation")
      elif value.endswith('manipulability'):
          raise NotImplementedError("TODO: do manipulability calculation")
      else:
          raise ValueError("Invalid value to output")
  
      points = []
      values = []
      if isinstance(constraint_or_point,IKObjective):
          #first do a run to figure out the possible cells        
          vg_temp,_,_ = _naive_workspace_occupancy(robot,link,point_local,Nsamples,
                  resolution,dimensions)
          bmin,bmax = [vg_temp.bbox[0],vg_temp.bbox[1],vg_temp.bbox[2]],[vg_temp.bbox[3],vg_temp.bbox[4],vg_temp.bbox[5]]
          cellsize = vectorops.div(vectorops.sub(bmax,bmin),vg_temp.dims)
          vals = vg_temp.getValues()
  
          #now solve the IK constraint
          i,j,k = vals.nonzero()
          for n in range(len(i)):
              ind = (i[n],j[n],k[n])
              target = vectorops.add(bmin,vectorops.mul(vectorops.add(ind,[0.5]*3),cellsize))  #target at center point
              #try reaching point described by ind. TODO: allow up to cellsize/2 slop
              constraint.setFixedPosConstraint(point_local,target)
              res = ik.solve_global(constraint,iters=20,numRestarts=5,feasibilityCheck=overall_feasibility_test)
              if res:
                  points.append(target)
                  values.append(value_fn())
  
          res = VolumeGrid(points,dimensions=vg_temp.dims,bounds=(bmin,bmax),value=value)
      else:
          vg_temp,points1,configs1 = _naive_workspace_occupancy(robot,link,point_local,Nsamples,
                  resolution,dimensions,overall_feasibility_test)
          vg_temp,points2,configs2 = _expand_workspace_occupancy(robot,link,point_local,vg_temp,overall_feasibility_test)
          for (p,q) in zip(points1+points2,configs1+configs2):
              robot.setConfig(q)
              if overall_feasibility_test():
                  points.append(p)
                  values.append(value_fn())
          res = compute_field_grid(points,values,resolution,dimensions,value=value)
  
      #restore original limits
      robot.setJointLimits(qmin_orig,qmax_orig)
  
      return res
function_tokens: def compute_workspace_field ( link : RobotModelLink , constraint_or_point : Union [ Vector3 , IKObjective ] , value : Union [ str , Callable ] , Nsamples = 100000 , resolution = 0.05 , dimensions = None , fixed_links : Sequence [ Union [ int , str ] ] = None , qmin : Config = None , qmax : Config = None , obstacles : Sequence [ Union [ RigidObjectModel , TerrainModel ] ] = None , load : Union [ float , Vector3 ] = None , load_type : str = None , gravity : Vector3 = ( 0 , 0 , - 9.8 ) , self_collision : bool = True , feasibility_test : Callable = None ) -> VolumeGrid : robot = link . robot ( ) qmin_orig , qmax_orig = robot . getJointLimits ( ) if qmin is None : qmin = qmin_orig else : if len ( qmin ) != len ( qmin_orig ) : raise ValueError ( "Invalid length of qmin" ) if qmax is None : qmax = qmax_orig else : if len ( qmax ) != len ( qmax_orig ) : raise ValueError ( "Invalid length of qmin" ) if fixed_links is not None : import copy q = robot . getConfig ( ) qmin = copy . copy ( qmin ) qmax = copy . copy ( qmax ) for l in fixed_links : i = robot . link ( l ) . getIndex ( ) qmin [ i ] = q [ i ] qmax [ i ] = q [ i ] robot . setJointLimits ( qmin , qmax ) else : robot . setJointLimits ( qmin , qmax ) if isinstance ( constraint_or_point , IKObjective ) : point_local , _ = constraint_or_point . getPosition ( ) constraint = constraint_or_point else : point_local = constraint_or_point constraint = None #setup feasibility test feasibleTests = _setup_feasible_tests ( robot , link , point_local , fixed_links , obstacles , load , load_type , gravity , self_collision , feasibility_test ) def overall_feasibility_test ( ) : return all ( test ( ) for k , test in feasibleTests . items ( ) ) if isinstance ( value , callable ) : value_fn = lambda : value ( robot . getConfig ( ) ) elif value == 'occupancy' : value_fn = lambda : 1 elif value == 'load' : assert 'load' in feasibleTests raise NotImplementedError ( "TODO: do load calculation" ) elif value . endswith ( 'manipulability' ) : raise NotImplementedError ( "TODO: do manipulability calculation" ) else : raise ValueError ( "Invalid value to output" ) points = [ ] values = [ ] if isinstance ( constraint_or_point , IKObjective ) : #first do a run to figure out the possible cells         vg_temp , _ , _ = _naive_workspace_occupancy ( robot , link , point_local , Nsamples , resolution , dimensions ) bmin , bmax = [ vg_temp . bbox [ 0 ] , vg_temp . bbox [ 1 ] , vg_temp . bbox [ 2 ] ] , [ vg_temp . bbox [ 3 ] , vg_temp . bbox [ 4 ] , vg_temp . bbox [ 5 ] ] cellsize = vectorops . div ( vectorops . sub ( bmax , bmin ) , vg_temp . dims ) vals = vg_temp . getValues ( ) #now solve the IK constraint i , j , k = vals . nonzero ( ) for n in range ( len ( i ) ) : ind = ( i [ n ] , j [ n ] , k [ n ] ) target = vectorops . add ( bmin , vectorops . mul ( vectorops . add ( ind , [ 0.5 ] * 3 ) , cellsize ) ) #target at center point #try reaching point described by ind. TODO: allow up to cellsize/2 slop constraint . setFixedPosConstraint ( point_local , target ) res = ik . solve_global ( constraint , iters = 20 , numRestarts = 5 , feasibilityCheck = overall_feasibility_test ) if res : points . append ( target ) values . append ( value_fn ( ) ) res = VolumeGrid ( points , dimensions = vg_temp . dims , bounds = ( bmin , bmax ) , value = value ) else : vg_temp , points1 , configs1 = _naive_workspace_occupancy ( robot , link , point_local , Nsamples , resolution , dimensions , overall_feasibility_test ) vg_temp , points2 , configs2 = _expand_workspace_occupancy ( robot , link , point_local , vg_temp , overall_feasibility_test ) for ( p , q ) in zip ( points1 + points2 , configs1 + configs2 ) : robot . setConfig ( q ) if overall_feasibility_test ( ) : points . append ( p ) values . append ( value_fn ( ) ) res = compute_field_grid ( points , values , resolution , dimensions , value = value ) #restore original limits robot . setJointLimits ( qmin_orig , qmax_orig ) return res