コード例 #1
0
def generateSubGoals(q0, edges):
    '''
  Generate a list of subgoals

  Input:
    - q0: initial configuration
    - edges: list of transitions
  Return
    - a list of subgoal configurations

  The first subgoal is the initial configuration q0. The following ones are
  generated by projecting random configurations on the destination node of
  each transition of the list. Each subgoal is reachable from the previous one
  by a path of the transition.
  '''
    subgoals = list()
    node = 'free'
    res, q_init, err = cg.applyNodeConstraints(node, q0)
    if not res:
        raise RuntimeError('Failed to project configuration on node ' + node)
    subgoals.append(q_init[::])
    ## Starting from initial configuration, iteratively produce random
    #  configurations on each successive node in such a way that each new
    #  configuration is reachable from the previous one through the transition
    #  linking the states of the configurations.
    q_init = subgoals[0]
    for edge in edges[:-1]:
        edgeSuccess = False
        for i in range(400):
            if i == 0:
                q = q_init
            else:
                q = robot.shootRandomConfig()
            res, q1, err = cg.generateTargetConfig(edge, q_init, q)
            if not res: continue
            res, msg = robot.isConfigValid(q1)
            if not res: continue
            v(q1)
            ps.addConfigToRoadmap(q1)
            subgoals.append(q1[::])
            q_init = q1[::]
            edgeSuccess = True
            break
        if not edgeSuccess:
            raise RuntimeError('Failed to generate a subgoal through edge ' +
                               edge)
    ## Generate last sub goal configuration to move back manipulator arms in
    #  initial configurations
    q_goal = subgoals[-1][::]
    q_goal[0:6] = q0_r0
    q_goal[6:12] = q0_r1
    subgoals.append(q_goal)
    return subgoals
コード例 #2
0
def generateSubGoals (q0, edges):
  '''
  Generate a list of subgoals

  Input:
    - q0: initial configuration
    - edges: list of transitions
  Return
    - a list of subgoal configurations

  The first subgoal is the initial configuration q0. The following ones are
  generated by projecting random configurations on the destination node of
  each transition of the list. Each subgoal is reachable from the previous one
  by a path of the transition.
  '''
  subgoals = list ()
  node = 'free'
  res, q_init, err = cg.applyNodeConstraints (node, q0)
  if not res:
    raise RuntimeError ('Failed to project configuration on node ' + node)
  subgoals.append (q_init [::])
  ## Starting from initial configuration, iteratively produce random
  #  configurations on each successive node in such a way that each new
  #  configuration is reachable from the previous one through the transition
  #  linking the states of the configurations.
  q_init = subgoals [0]
  for edge in edges [:-1]:
    edgeSuccess = False
    for i in range (400):
      if i == 0:
        q = q_init
      else:
        q = robot.shootRandomConfig ()
      res, q1, err = cg.generateTargetConfig (edge, q_init, q)
      if not res: continue
      res, msg = robot.isConfigValid (q1)
      if not res: continue
      v (q1)
      ps.addConfigToRoadmap (q1)
      subgoals.append (q1 [::])
      q_init = q1 [::]
      edgeSuccess = True
      break
    if not edgeSuccess:
      raise RuntimeError ('Failed to generate a subgoal through edge ' + edge)
  ## Generate last sub goal configuration to move back manipulator arms in
  #  initial configurations
  q_goal = subgoals [-1] [::]
  q_goal [0:6] = q0_r0; q_goal [6:12] = q0_r1
  subgoals.append (q_goal)
  return subgoals