예제 #1
0
def gamee_solver(firstnode): 
    found = False
    allsteps = []
    movesTree = Tree()
    movesTree.add_firstnode(firstnode)

    start_time = time.time()
    
    if getInversionCount(firstnode) %2 !=  getInversionCount(goal) %2:
        return allsteps, (time.time() - start_time)
    
    loopQueue = qu.PriorityQueue()
    loopQueue.put(mhd(firstnode)+firstnode)
    
    while not loopQueue.empty():
        node = loopQueue.get()
        node = node[2:]
        if found == True:
            break        
        if check_goal(node) == True:
            allsteps = findSteps(node, movesTree)
            found = True
            break
        else:
            for child in move(node):
                if check_goal(child) == True:
                    allsteps = findSteps(node, movesTree)
                    found = True
                    break
                elif child not in movesTree.nodes():
                    movesTree.add_node(child, node)
                    loopQueue.put(mhd(child)+child)
                    
    return allsteps, (time.time() - start_time)
예제 #2
0
def gamee_solver(firstnode):
    found = False
    allsteps = []
    movesTree = Tree()
    movesTree.add_firstnode(firstnode)

    start_time = time.time()
    if getInversionCount(firstnode) % 2 != getInversionCount(goal) % 2:
        return allsteps, (time.time() - start_time)

    for node in movesTree.traverse(firstnode):
        if found == True:
            break
        if check_goal(node) == True:
            allsteps = findSteps(node, movesTree)
            found = True
            break
        else:
            for child in move(node):
                if check_goal(child) == True:
                    allsteps = findSteps(node, movesTree)
                    found = True
                    break
                elif child not in movesTree.nodes():
                    movesTree.add_node(child, node)
    return allsteps, (time.time() - start_time)
예제 #3
0
class DicomSR(object):
    def __init__(self, report_type="", id_ontology=-1):
        self.report_type = report_type
        self.id_ontology = id_ontology
        self.report = Tree()

    def imprime(self):
        """ Pretty print of a report """
        print u"\n ------ {0} ---------- \n".format(self.report_type)
        self.report.print_tree(0)

    def add_node(self, node, parent):
        self.report.add_node(node, parent)

    def get_ontology(self):
        """ Return current report ontology """
        return self.id_ontology

    def get_flat_data(self):
        flat = {}
        self.report.get_flat_tree(flat)
        return flat

    def get_root(self):
        return self.report.value

    def get_data_from_report(self, template_type, languages=None,
                             position=None, cardinality=None):
        """ Return data from the report in a dictionary

        Keyword arguments:
        languages -- list of languages supported in the  report.
        template_type -- indicates the template type and
                         therefore the information to extract from the report.
        self -- Dict Report with the information extracted from the dicom XML.
        position -- dictionary where it's stored parent and
                    its children position

        """
        substitution_words = {}
        if (template_type in MULTIPLE_PROPERTIES.keys()):
            if (template_type == DICOM_LEVEL):
                # Get keys for this template
                levels_tag = MULTIPLE_PROPERTIES[DICOM_LEVEL][0]
                attrs_tag = MULTIPLE_PROPERTIES[DICOM_LEVEL][1]
                level_name = MULTIPLE_PROPERTIES[DICOM_LEVEL][3]
                level_num = MULTIPLE_PROPERTIES[DICOM_LEVEL][2]
                code = MULTIPLE_PROPERTIES[DICOM_LEVEL][4]
                meaning = MULTIPLE_PROPERTIES[DICOM_LEVEL][5]
                #Init dictinary will hold the substitution.
                # Keys are language code and values  contains
                # values to fill the template
                for language in languages:
                    substitution_words[language] = {
                        levels_tag: [],
                        attrs_tag: []}
                #Get container's concept and its attributes
                containers = []
                attributes = []
                self.report.get_set_data(containers, attributes)
                for container in containers:
                    ontology = get_ontology_level(
                        ontology_id=self.get_ontology(),
                        tree_level=container.get_level(),
                        languages_tag=language)
                    for language in languages:
                        aux = {}
                        aux[level_num] = container.get_level()
                        aux[level_name] = (unicode(ontology, "utf-8"))
                        aux[code] = container.get_code().lower()
                        aux[meaning] = container.get_meaning()[language]
                        substitution_words[language][levels_tag].\
                            append(aux.copy())
                for attribute in attributes:
                    for language in languages:
                        aux = {}
                        aux[code] = attribute.code.lower()
                        aux[meaning] = attribute.meaning[language]
                        substitution_words[language][attrs_tag].\
                            append(aux.copy())

            elif (template_type == CHILDREN_ARRAYS):
                nodes_tag = MULTIPLE_PROPERTIES[CHILDREN_ARRAYS][0]
                parent_tag = MULTIPLE_PROPERTIES[CHILDREN_ARRAYS][1]
                children_tag = MULTIPLE_PROPERTIES[CHILDREN_ARRAYS][2]
                # Get a flat version of the report
                # TODO: check if the flat version of the tree is
                # always the same
                flat = {}
                self.report.get_flat_tree(flat)
                #Delete leaf items. They are not needed
                flat = {key: flat[key] for key in flat if flat[key]}
                if languages:
                    for language in languages:
                        substitution_words[language] = {nodes_tag: []}

                    for parent, children in flat.iteritems():
                        for language in languages:
                            aux = {parent_tag: parent.get_code().lower(),
                                   children_tag: []}
                            position = 0
                            for child in children:
                                aux[children_tag].append(
                                    child.get_meaning()[language])
                                #print parent.get_schema_code()
                                #print child.get_schema_code(), position
                                position += 1
                            substitution_words[language][nodes_tag].append(aux)
                else:
                    for parent, children in flat.iteritems():
                        p_code = (parent.get_schema().lower() + '_' 
                                  + parent.get_code().lower())
                        position[p_code] = {}
                        cardinality[p_code] = {}
                        pos = 0
                        for child in children:
                            position[p_code][pos] = child.get_schema_code()
                            cardinality[p_code][pos] = child.get_max_cardinality()
                            pos += 1

            elif (template_type == CODE_ARRAYS):
                #TODO: Change comment on this template to make it different
                #from CHILDREN_ARRAYS
                nodes_tag = MULTIPLE_PROPERTIES[CODE_ARRAYS][0]
                parent_tag = MULTIPLE_PROPERTIES[CODE_ARRAYS][1]
                children_tag = MULTIPLE_PROPERTIES[CODE_ARRAYS][2]
                for language in languages:
                    substitution_words[language] = {nodes_tag: []}
                codes = self.report.get_code_containers()
                for code in codes:
                    for language in languages:
                        aux = {parent_tag: code.code.lower(), children_tag: []}
                        for option in code.options:
                            aux[children_tag].append(
                                option.meaning[language])
                        substitution_words[language][nodes_tag].append(aux)

        return substitution_words
from tree import Tree
from tree import Node

myTree = Tree()
#print(myTree.get_root())

n = Node('taste',5)
p = Node('var',6)
q = Node('var',7)
r = Node('var',8)
s = Node('name',9)

myTree.add_node(n,myTree.get_root())
print("Traversing the tree after adding 1 node")
myTree.print_tree(myTree.get_root(),0)

myTree.add_node(p,myTree.search_node(myTree.get_root(),n.feature,n.value))
print("Traversing the tree after adding 2 nodes")
myTree.print_tree(myTree.get_root(),0)
myTree.add_node(q,myTree.search_node(myTree.get_root(),n.feature,n.value))
myTree.add_node(r,myTree.search_node(myTree.get_root(),q.feature,q.value))

print("Traversing the tree after adding 4 nodes")
myTree.print_tree(myTree.get_root(),0)
myTree.add_node(s,myTree.search_node(myTree.get_root(),r.feature,r.value))

"""
n.add_child(p)
n.add_child(q)
n.add_child(r)
r.add_child(s)
예제 #5
0
    if key1 in move(key2):
        return True
    else:
        return False


########################################################
if getInversionCount(firstnode) % 2 != getInversionCount(goal) % 2:
    print("There is no solution for the input puzzle")
    sys.exit()

### builds a complete tree from goal as root.
for node in movesTree.traverse(goal):
    for child in move(node):
        if child not in movesTree.nodes():
            movesTree.add_node(child, node)

print(
    "Problem 1. The expression for total number of possible stating states is : 9!\n"
)

maxSteps = movesTree.maxSteps(goal)
print("The hardest puzzle for solving the goal 12345678_ needs = ", maxSteps)

pzlSteps = findSteps(firstnode)
print("\nProblem 2. The maxSteps between input puzzle ", puzzle,
      " and the goal is: ", len(pzlSteps))

print("\nProblem 3. For goal 12345678_, we have 1 to ", maxSteps,
      " different steps:")
distanceSteps = []
예제 #6
0
파일: tree.py 프로젝트: crousseau/lasagna
        else:
            return False


# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#Generate an example if run from the command line
if __name__ == '__main__':

    from tree import Tree

    (_ROOT, _DEPTH, _WIDTH) = list(range(3))

    print("\n\n   --------- Tree of life --------- \n")
    treeOfLife = Tree()

    treeOfLife.add_node("Life")  # root node
    treeOfLife.add_node("Archaebacteria", "Life")
    treeOfLife.add_node("Eukaryotes", "Life")
    treeOfLife.add_node("Protista", "Eukaryotes")
    treeOfLife.add_node("Plants", "Eukaryotes")
    treeOfLife.add_node("Fungi", "Eukaryotes")
    treeOfLife.add_node("Algae", "Plants")
    treeOfLife.add_node("Mosses", "Plants")
    treeOfLife.add_node("Ferns", "Plants")
    treeOfLife.add_node("Animals", "Eukaryotes")
    treeOfLife.add_node("Sponges", "Animals")
    treeOfLife.add_node("Flatworms", "Animals")
    treeOfLife.add_node("Arthropods", "Animals")
    treeOfLife.add_node("Insects", "Arthropods")
    treeOfLife.add_node("Crustaceans", "Arthropods")
    treeOfLife.add_node("Vertebrates", "Animals")
      MLBD_all = []
      # record the chosen nodes with the lowest MLBD of each iteration
      nodes_all = []
      # record the time for each iteration
      time_iteration = []
 	
      print "Start optimizing..."
      
      index=0
      global num_node
      num_node = 0
      current_node = 0
      
      #Claim a tree
      tree = Tree()
      node = tree.add_node(current_node, theta_L, theta_U)
      num_node = num_node + 1 
  
      start_all = time.clock()
      print x_all[-1]
      
      while index < MAXITER:
          
          start = time.clock()
          print "----------------------------iteration %d---------------------------" % index
          
          # Solve the subproblem
          objOpt, thetaOpt, lamOpt, muOpt = sub.solve_subproblem(y, xBar)   #(objOpt) upper bound
  
          thetaBar.append(thetaOpt)
          lamBar.append(lamOpt)
예제 #8
0
cursor = cnx.cursor()

query = ("SELECT * FROM imm_web_node_relation ")

cursor.execute(query)

(_ROOT, _DEPTH, _BREADTH) = range(3)

tree = Tree()
for (parent, child) in cursor:
    print("parent: {}, child: {}".format(parent, child))
    
    try:
        tree[parent]
    except KeyError:
        tree.add_node(parent)
        
    try:
        tree[child]
    except KeyError:
        tree.add_node(child)
        
    tree[parent].add_child(child)
    tree[child].add_parent(parent)


query = ("SELECT id, name FROM imm_web")
cursor.execute(query)
id_name_dict = {}
for (id, name) in cursor:
    id_name_dict[id] = name
예제 #9
0
def run_ros():
    rospy.init_node('ros_demo')

    # First param: topic name; second param: type of the message to be published; third param: size of queued messages,
    # at least 1
    chatter_pub = rospy.Publisher('some_chatter', String, queue_size=10)
    marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)

    # Each second, ros "spins" and draws 20 frames
    loop_rate = rospy.Rate(20)  # 20hz

    frame_count = 0

    obstacles = create_obstacles()

    obstacles_lines = get_obstacles_lines(obstacles)

    robot = create_robot()

    target = create_target()

    target_lines = get_target_lines(target)

    point = get_point_structure()

    tree_edges = get_tree_edges_structure()

    p0 = Point(robot.pose.position.x, robot.pose.position.y, 0)

    tree = Tree(TreeNode(p0))

    collision_edges = get_collision_edges_structure()

    found_path = False

    path_edges = get_path_edges_structure()

    drawed_path = False

    robot_reached = False

    path_points = []

    while not rospy.is_shutdown():
        msg = "Frame index: %s" % frame_count

        rospy.loginfo(msg)

        chatter_pub.publish(msg)

        for obst in obstacles:
            obst.header.stamp = rospy.Time.now()
            marker_pub.publish(obst)

        robot.header.stamp = rospy.Time.now()

        target.header.stamp = rospy.Time.now()
        marker_pub.publish(target)

        point.header.stamp = rospy.Time.now()

        tree_edges.header.stamp = rospy.Time.now()

        collision_edges.header.stamp = rospy.Time.now()

        path_edges.header.stamp = rospy.Time.now()

        if frame_count % HZ == 0 and not found_path:
            rand_pnt = Point()
            rand_pnt.x = random.uniform(BOARD_CORNERS[0], BOARD_CORNERS[1])
            rand_pnt.y = random.uniform(BOARD_CORNERS[3], BOARD_CORNERS[2])
            rand_pnt.z = 0
            point.points = [rand_pnt]

            close_node = tree.get_closest_node(rand_pnt)
            close_pnt = close_node.point

            # https://math.stackexchange.com/questions/175896/finding-a-point-along-a-line-a-certain-distance-away-from-another-point

            total_dist = math.sqrt(
                math.pow(rand_pnt.x - close_pnt.x, 2) +
                math.pow(rand_pnt.y - close_pnt.y, 2))

            dist_ratio = STEP / total_dist

            new_pnt = Point()
            new_pnt.x = (1 -
                         dist_ratio) * close_pnt.x + dist_ratio * rand_pnt.x
            new_pnt.y = (1 -
                         dist_ratio) * close_pnt.y + dist_ratio * rand_pnt.y
            new_pnt.z = 0

            if collides_object(close_pnt, new_pnt, obstacles_lines):
                collision_edges.points.append(close_pnt)
                collision_edges.points.append(new_pnt)
            else:
                last_node = tree.add_node(close_node, new_pnt)

                tree_edges.points.append(close_pnt)
                tree_edges.points.append(new_pnt)

            if collides_object(close_pnt, new_pnt, target_lines):
                found_path = True

        if found_path and not drawed_path:
            current_node = last_node
            while not current_node.is_root():
                path_points.append(current_node.point)
                path_edges.points.append(current_node.point)
                path_edges.points.append(current_node.parent.point)
                current_node = current_node.parent
            drawed_path = True

        if frame_count % 2 == 0 and drawed_path and not robot_reached:
            robot.pose.position = path_points.pop()
            robot_reached = True if len(path_points) == 0 else False

        marker_pub.publish(robot)
        marker_pub.publish(point)
        marker_pub.publish(tree_edges)
        marker_pub.publish(collision_edges)
        marker_pub.publish(path_edges)

        # TODO: Robot function
        """

        # From here, we are defining and drawing a simple robot

        rob.type = rob.SPHERE
        path.type = path.LINE_STRIP

        rob.header.frame_id = "map"
        path.header.frame_id = "map"

        rob.header.stamp = rospy.Time.now()
        path.header.stamp = rospy.Time.now()

        rob.ns = "rob"
        path.ns = "rob"

        rob.id = 0
        path.id = 1

        rob.action = rob.ADD
        path.action = path.ADD

        rob.lifetime = rospy.Duration()
        path.lifetime = rospy.Duration()

        rob.scale.x, rob.scale.y, rob.scale.z = 0.3, 0.3, 0.3

        rob.color.r, rob.color.g, rob.color.b, rob.color.a = 1.0, 0.5, 0.5, 1.0

        # Path line strip is blue
        path.color.b, path.color.a = 1.0, 1.0

        path.scale.x = 0.02
        path.pose.orientation.w = 1.0

        num_slice2 = 200 # Divide a circle into segments

        if frame_count % 2 == 0 and len(path.points) <= num_slice2:
            p = Point()

            angle = slice_index2 * 2 * math.pi / num_slice2
            slice_index2 += 1
            p.x = 4 * math.cos(angle) - 0.5 # Some random circular trajectory, with radius 4, and offset (-0.5, 1, .05)
            p.y = 4 * math.sin(angle) + 1.0
            p.z = 0.05

            rob.pose.position = p
            path.points.append(p) # For drawing path, which is line strip type

        marker_pub.publish(rob)
        marker_pub.publish(path)
        
        """

        # To here, we finished displaying our components

        # Check if there is a subscriber. Here our subscriber will be Rviz
        while marker_pub.get_num_connections() < 1:
            if rospy.is_shutdown():
                return 0
            rospy.logwarn_once("Please run Rviz in another terminal.")
            rospy.sleep(1)

        loop_rate.sleep()
        frame_count += 1
예제 #10
0
파일: tree.py 프로젝트: raacampbell/lasagna



# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#Generate an example if run from the command line
if __name__ == '__main__':

    from tree import Tree

    (_ROOT, _DEPTH, _WIDTH) = range(3)

    print "\n\n   --------- Tree of life --------- \n"
    treeOfLife = Tree()

    treeOfLife.add_node("Life")  # root node
    treeOfLife.add_node("Archaebacteria", "Life")
    treeOfLife.add_node("Eukaryotes", "Life")
    treeOfLife.add_node("Protista", "Eukaryotes")
    treeOfLife.add_node("Plants", "Eukaryotes")
    treeOfLife.add_node("Fungi", "Eukaryotes")
    treeOfLife.add_node("Algae", "Plants")
    treeOfLife.add_node("Mosses", "Plants")
    treeOfLife.add_node("Ferns", "Plants")
    treeOfLife.add_node("Animals", "Eukaryotes")
    treeOfLife.add_node("Sponges","Animals")
    treeOfLife.add_node("Flatworms","Animals")
    treeOfLife.add_node("Arthropods","Animals")
    treeOfLife.add_node("Insects","Arthropods")
    treeOfLife.add_node("Crustaceans","Arthropods")
    treeOfLife.add_node("Vertebrates","Animals")
예제 #11
0
def main():
    tree = Tree()

    #set root node
    root = "Forest"

    mode = input(
        'ENTER 0 FOR DFS, ENTER 1 FOR BFS\nNOTE: Strings must be between quotes\n'
    )
    #print(mode)
    #print(mode == 1)

    #add elements to the graph
    #USAGE: tree.add_node(leaf,parent)

    tree.add_node(root)
    tree.add_node("Steve", root)
    tree.add_node("Kao", root)
    tree.add_node("Diplo", root)
    tree.add_node("Lol", "Steve")
    tree.add_node("Amy", "Steve")
    tree.add_node("Julio", "Amy")
    tree.add_node("Mary", "Amy")
    tree.add_node("Mark", "Julio")
    tree.add_node("Jane", "Mark")
    tree.add_node("Tahir", "Kao")
    tree.add_node("What", "Tahir")

    tree.display(root)

    if (mode == 0):
        print("DEPTH-FIRST ITERATION:\n")
        for node in tree.traverse(root):
            tree.specialdisplay(root, node)
            print("\n")
            time.sleep(1)
    if (mode == 1):
        print("BREADTH-FIRST ITERATION:\n")
        for node in tree.traverse(root, mode=BREADTH):
            tree.specialdisplay(root, node)
            print("\n")
            time.sleep(1)

    restart = input('RESTART? ("y" or "n")\n')
    if (restart == 'y' or restart == 'Y'):
        main()
예제 #12
0
n = Node('taste')
n.add_value('o')

p = Node('var')
n.add_value('a')

q = Node('var')
n.add_value('b')

r = Node('var')
r.add_value('c')

s = Node('name')

myTree.add_node(n, myTree.get_root())
print("Traversing the tree after adding 1 node")
myTree.print_tree(myTree.get_root(), 0)

myTree.add_node(p, n)
#myTree.add_node(p,myTree.search_node(myTree.get_root(),n.feature,n.value))
print("Traversing the tree after adding 2 nodes")
myTree.print_tree(myTree.get_root(), 0)
myTree.add_node(q, n)
myTree.add_node(r, n)

print("Traversing the tree after adding 4 nodes")
myTree.print_tree(myTree.get_root(), 0)
myTree.add_node(s, r)
"""
n.add_child(p)
예제 #13
0
(_ROOT, _DEPTH, _BREADTH) = range(3)
'''

            0
    1               2
3       4       5       6





'''

tree = Tree()

tree.add_node("0")  # root node
tree.add_node("1", "0")
tree.add_node("2", "0")
tree.add_node("3", "1")
tree.add_node("4", "1")
tree.add_node("5", "2")
tree.add_node("6", "2")
tree.add_node("7", "6")
tree.add_node("8", "6")
tree.add_node("9", "5")
tree.add_node("10", "5")
tree.display("0")
'''
print("***** DEPTH-FIRST ITERATION *****")
for node in tree.traverse("Harry"):
    print(node)
예제 #14
0
from tree_node import TreeNode
from tree import Tree


if __name__ == "__main__":

    n0 = TreeNode("Root")
    n1 = TreeNode('N1')
    n2 = TreeNode('N2')
    n3 = TreeNode('N3')
    n0.right = n1
    n0.left = n2
    n1.left = n3

    #print(n1.is_leaf())
    #TreeNode.print_tree(n0)

    tree_display = Tree(n0)
    tree_display.transversal_deep()

    tree_display.add_node("NEW", n3)
    tree_display.transversal_deep()
    # tree_display.add_node("another_node", n1)
    # tree_display.transversal_deep()

    tree_display.remove_node(n1)
    tree_display.transversal_deep()
    tree_display.remove_node(n2)
    tree_display.transversal_deep()
예제 #15
0
li = {
    'c': 'C',
    'h': 'H',
    'a': 'A',
    'e': 'E',
    'f': 'F',
    'd': 'D',
    'b': 'B',
    'j': 'J',
    'g': 'G',
    'i': 'I',
    'k': 'K'
}
for key, value in li.items():
    t.add_node(key, value)

print('Initial tree:')
t.debug_print()

print('\nLookups:')
print(t.get('f').value)
print(t.get('b').value)
print(t.get('i').value)

print('\nBFS:')
print(t.walk_bfs())

print('\nDFS preorder:')
print(t.walk_dfs_preorder())
def main():
    t = Tree(1)
    t.add_node(t.root, 2)
    three_node = t.add_node(t.root, 3)
    four_node = t.add_node(t.root, 4)
    t.add_node(t.root, 5)
    t.add_node(three_node, 6)
    t.add_node(three_node, 7)
    t.add_node(three_node, 8)
    t.add_node(four_node, 9)
    t.add_node(four_node, 10)

    t.depth_first_traversal()
    print("")
    t.breadth_first_traversal()
    print("")
예제 #17
0
파일: tree.py 프로젝트: raacampbell/lasagna
def importData(fname, displayTree=False, colSep=',', headerLine=False, verbose=False):
    """
    Import tree data from a CSV (text) file or list. 

    The data should be in the following format:
    node_ID_number,node_parent_ID_number,data_item1,data_item2,...,data_itemN\n

    The separator can, optionally, be a character other than ","

    The root node must have a parent id of 0 and normally should also have an index of 1

    From MATLAB one can produce tree structures and dump data in the correct format
    using https://github.com/raacampbell13/matlab-tree and the tree.dumptree method

    Inputs:
    fname - if a string, importData assumes it is a file name and tries to load the tree from file.
            if it is a list, importData assumes that each line is a CSV data line and tries to 
            convert to a tree.        
    displayTree - if True the tree is printed to standard output after creation
    colSep - the data separator, a comma by default.
    headerLine - if True, the first line is stripped off and considered to be the column headings.
                headerLine can also be a CSV string or a list that defines the column headings. Must have the
                same number of columns as the rest of the file.
    verbose - prints diagnositic info to screen if true
    """



    if verbose:
        print "tree.importData importing file %s" % fname

    #Error check
    if isinstance(fname,str):
        if os.path.exists(fname)==False:
            print "Can not find file " + fname
            return

        #Read in data
        fid = open(fname,'r')
        contents = fid.read().split('\n')
        fid.close()

    elif isinstance(fname,list):
        contents=fname #assume that fname is data rather than a file name


    #Get header data if present
    if headerLine==True:
        header = contents.pop(0)
        header = header.rstrip('\n').split(colSep)
    elif isinstance(headerLine,str):
        header = headerLine.rstrip('\n').split(colSep)
    elif isinstance(headerLine,list):
        header = headerLine
    else:
        header = False


    data = []
    for line in contents:
        if len(line)==0:
            continue

        dataLine = line.split(colSep)
        if len(header) !=len(dataLine):
            print "\nTree file appears corrupt! header length is %d but data line length is %d.\ntree.importData is aborting.\n" % (len(header),len(dataLine))
            return False

        theseData = map(int,dataLine[0:2]) #add index and parent to the first two columns

        #Add data to the third column. Either as a list or as a dictionary (if header names were provided)
        if header != False: #add as dictionary
            dataCol = dict()

            for ii in range(len(header)-2):
                ii+=2
                dataCol[header[ii]]=dataTypeFromString.convertString(dataLine[ii])

        else:
            dataCol = dataLine[2:] #add as list of strings


        theseData.append(dataCol) 
        data.append(theseData)

    if verbose:
        print "tree.importData read %d rows of data from %s" % (len(data),fname)


    #Build tree
    tree = Tree()
    tree.add_node(0)
    for thisNode in data:
        tree.add_node(thisNode[0],thisNode[1])
        tree[thisNode[0]].data = thisNode[2]


    #Optionally dump the tree to screen (unlikely to be useful for large trees)
    if displayTree:
        tree.display(0)

        for nodeID in tree.traverse(0):
            print "%s - %s" % (nodeID, tree[nodeID].data)

    return tree
예제 #18
0
##                print ("loop_body: ", loop_body)
#        temp_body = temp_body * 2
    temp_i = loop_start[i] + 1
    if "pthread_cond" in temp_body[0]:
        #print ("+++++++")
        break
    else:
        for j in range(0, len(temp_body)):
            info.insert(temp_i, temp_body[j])
            temp_i += 1

    gap = len(info) - info_length
    temp_body = []
#print ("fixing info: ", info)

tree.add_node(info[treeID])
for i in range(1, len(info)):
    #info[i] = info[i] + "/*R2 line:" + str(treeID) + "*/"
    info[i] = "/*R2 line:" + str(treeID) + "*/" + info[i]
    if (("if" in info[i]) and ("else" not in info[i])):
        brackets_match += 1
        cond_num.append(brackets_match)
        branch_layer.append(brackets_match)
        branch_point.append(treeID)
        #                if ("else" not in  info[i-1]):
        if cond_num[len(cond_num) - 1] == cond_num[len(cond_num) - 2]:
            temp = branch_point[brackets_match - 1]
            tree.add_node(info[i], info[temp])
            p_num.append(temp)
            cond_list.append(temp)
            cond_final_list.append(temp)
예제 #19
0
class DicomSR(object):
    def __init__(self, report_type="", id_ontology=-1):
        self.report_type = report_type
        self.id_ontology = id_ontology
        self.report = Tree()

    def imprime(self):
        """ Pretty print of a report """
        print u"\n ------ {0} ---------- \n".format(self.report_type)
        self.report.print_tree(0)

    def add_node(self, node, parent):
        self.report.add_node(node, parent)

    def get_ontology(self):
        """ Return current report ontology """
        return self.id_ontology

    def get_flat_data(self):
        flat = {}
        self.report.get_flat_tree(flat)
        return flat

    def get_root(self):
        return self.report.value

    def get_data_from_report(self,
                             template_type,
                             languages=None,
                             position=None,
                             cardinality=None):
        """ Return data from the report in a dictionary

        Keyword arguments:
        languages -- list of languages supported in the  report.
        template_type -- indicates the template type and
                         therefore the information to extract from the report.
        self -- Dict Report with the information extracted from the dicom XML.
        position -- dictionary where it's stored parent and
                    its children position

        """
        substitution_words = {}
        if (template_type in MULTIPLE_PROPERTIES.keys()):
            if (template_type == DICOM_LEVEL):
                # Get keys for this template
                levels_tag = MULTIPLE_PROPERTIES[DICOM_LEVEL][0]
                attrs_tag = MULTIPLE_PROPERTIES[DICOM_LEVEL][1]
                level_name = MULTIPLE_PROPERTIES[DICOM_LEVEL][3]
                level_num = MULTIPLE_PROPERTIES[DICOM_LEVEL][2]
                code = MULTIPLE_PROPERTIES[DICOM_LEVEL][4]
                meaning = MULTIPLE_PROPERTIES[DICOM_LEVEL][5]
                #Init dictinary will hold the substitution.
                # Keys are language code and values  contains
                # values to fill the template
                for language in languages:
                    substitution_words[language] = {
                        levels_tag: [],
                        attrs_tag: []
                    }
                #Get container's concept and its attributes
                containers = []
                attributes = []
                self.report.get_set_data(containers, attributes)
                for container in containers:
                    ontology = get_ontology_level(
                        ontology_id=self.get_ontology(),
                        tree_level=container.get_level(),
                        languages_tag=language)
                    for language in languages:
                        aux = {}
                        aux[level_num] = container.get_level()
                        aux[level_name] = (unicode(ontology, "utf-8"))
                        aux[code] = container.get_code().lower()
                        aux[meaning] = container.get_meaning()[language]
                        substitution_words[language][levels_tag].\
                            append(aux.copy())
                for attribute in attributes:
                    for language in languages:
                        aux = {}
                        aux[code] = attribute.code.lower()
                        aux[meaning] = attribute.meaning[language]
                        substitution_words[language][attrs_tag].\
                            append(aux.copy())

            elif (template_type == CHILDREN_ARRAYS):
                nodes_tag = MULTIPLE_PROPERTIES[CHILDREN_ARRAYS][0]
                parent_tag = MULTIPLE_PROPERTIES[CHILDREN_ARRAYS][1]
                children_tag = MULTIPLE_PROPERTIES[CHILDREN_ARRAYS][2]
                # Get a flat version of the report
                # TODO: check if the flat version of the tree is
                # always the same
                flat = {}
                self.report.get_flat_tree(flat)
                #Delete leaf items. They are not needed
                flat = {key: flat[key] for key in flat if flat[key]}
                if languages:
                    for language in languages:
                        substitution_words[language] = {nodes_tag: []}

                    for parent, children in flat.iteritems():
                        for language in languages:
                            aux = {
                                parent_tag: parent.get_code().lower(),
                                children_tag: []
                            }
                            position = 0
                            for child in children:
                                aux[children_tag].append(
                                    child.get_meaning()[language])
                                #print parent.get_schema_code()
                                #print child.get_schema_code(), position
                                position += 1
                            substitution_words[language][nodes_tag].append(aux)
                else:
                    for parent, children in flat.iteritems():
                        p_code = (parent.get_schema().lower() + '_' +
                                  parent.get_code().lower())
                        position[p_code] = {}
                        cardinality[p_code] = {}
                        pos = 0
                        for child in children:
                            position[p_code][pos] = child.get_schema_code()
                            cardinality[p_code][
                                pos] = child.get_max_cardinality()
                            pos += 1

            elif (template_type == CODE_ARRAYS):
                #TODO: Change comment on this template to make it different
                #from CHILDREN_ARRAYS
                nodes_tag = MULTIPLE_PROPERTIES[CODE_ARRAYS][0]
                parent_tag = MULTIPLE_PROPERTIES[CODE_ARRAYS][1]
                children_tag = MULTIPLE_PROPERTIES[CODE_ARRAYS][2]
                for language in languages:
                    substitution_words[language] = {nodes_tag: []}
                codes = self.report.get_code_containers()
                for code in codes:
                    for language in languages:
                        aux = {parent_tag: code.code.lower(), children_tag: []}
                        for option in code.options:
                            aux[children_tag].append(option.meaning[language])
                        substitution_words[language][nodes_tag].append(aux)

        return substitution_words
예제 #20
0
     children = []
     for char in user_input[4:]:
         if char in symbol_set:
             current_item += char
         elif char == ':':
             parent = current_item
             current_item = ''
         elif char == ',':
             children.append(current_item)
             current_item = ''
     if current_item != '':
         children.append(current_item)
         current_item = ''
     if parent != '':
         for child in children:
             tree.add_node(parent, child)
 elif len(user_input) > 4 and user_input[0:5] == 'print':
     if tree == None:
         print('Дерево пусте!\n')
         continue
     print(tree.root)
 elif len(user_input) > 8 and user_input[0:9] == 'postorder':
     if tree == None:
         print('Дерево пусте!\n')
         continue
     print_list(tree.get_postorder_list())
 elif len(user_input) > 7 and user_input[0:8] == 'preorder':
     if tree == None:
         print('Дерево пусте!\n')
         continue
     print_list(tree.get_preorder_list())
예제 #21
0
# Copyright (C) by Brett Kromkamp 2011-2014 ([email protected])
# You Programming (http://www.youprogramming.com)
# May 03, 2014

from tree import Tree

(_ROOT, _DEPTH, _BREADTH) = range(3)

tree = Tree()

tree.add_node("Harry")  # root node
tree.add_node("Jane", "Harry")
tree.add_node("Bill", "Harry")
tree.add_node("Joe", "Jane")
tree.add_node("Diane", "Jane")
tree.add_node("George", "Diane")
tree.add_node("Mary", "Diane")
tree.add_node("Jill", "George")
tree.add_node("Carol", "Jill")
tree.add_node("Grace", "Bill")
tree.add_node("Mark", "Jane")

tree.display("Harry")
print("***** DEPTH-FIRST ITERATION *****")
for node in tree.traverse("Harry"):
    print(node)
print("***** BREADTH-FIRST ITERATION *****")
for node in tree.traverse("Harry", mode=_BREADTH):
    print(node)
예제 #22
0
from tree import Tree

tree = Tree()

tree.add_node(1)  # root node
tree.add_node(2,3)  # root node
tree.add_node(4,5)  # root node
tree.add_node(6,7)  # root node

tree.display(1)
예제 #23
0
def readJobs(disk, cpu):
    jobs = []
    with open(sys.argv[3], "r") as file:
        n_segments = 0
        ios = []

        # Lista de Jobs ordenada pelo tempo de início
        for line in file:
            segment_tree = Tree()
            line = line.split("\n")[0]
            items = line.split(" ")
            n_io = int(items[2])
            n_segments = int(items[5])
            # Organiza segmentos do job
            for i in range(0, n_segments):
                line = next(file)
                line = line.split("\n")[0]
                items_segment = line.split(" ")
                if len(items_segment) == 2:
                    segment_tree.add_node(int(items_segment[0]),
                                          items_segment[1])
                else:
                    segment_tree.add_node(int(items_segment[0]),
                                          items_segment[1],
                                          int(items_segment[2]))
            job = Job(items[0], items[1], None, items[3], items[4],
                      segment_tree)
            # Organiza IOs
            for i in range(0, n_io):
                line = next(file)
                line = line.split("\n")[0]
                items_io = line.split(" ")
                io_name = items_io[0]
                if io_name == DeviceType.Printer:
                    ios.append(
                        Device(
                            DeviceType.Printer,
                            int(
                                random.normal(cpu.TIMESLICE / 2,
                                              cpu.TIMESLICE / 10))))
                elif io_name == DeviceType.Reader:
                    ios.append(
                        Device(
                            DeviceType.Reader,
                            int(
                                random.normal(cpu.TIMESLICE / 2,
                                              cpu.TIMESLICE / 10))))
                elif io_name == DeviceType.Disk:
                    filename = items_io[1]
                    if items_io[2] == "r":
                        read_write = DiskOperation.Read
                    else:
                        read_write = DiskOperation.Write
                    n_opers = int(items_io[3])
                    size = int(items_io[4])
                    is_private = items_io[5]
                    sys_file = FileSystem(filename, job, size, is_private)
                    time = int(
                        random.normal(cpu.TIMESLICE / 2, cpu.TIMESLICE / 10))
                    ios.append(
                        Device(DeviceType.Disk, time, sys_file, read_write,
                               n_opers))
                    disk.addFile(sys_file)
            job.ios = ios
            # Coloca o job no segmento para facilitar impressao da memoria
            for i in range(0, segment_tree.size):
                segment_tree.__getitem__(i).job = job
            jobs.append(job)
            ios = []
    printJobs(jobs)
    return jobs
예제 #24
0
파일: tree.py 프로젝트: crousseau/lasagna
def importData(fname,
               displayTree=False,
               colSep=',',
               headerLine=False,
               verbose=False):
    """
    Import tree data from a CSV (text) file or list. 

    The data should be in the following format:
    node_ID_number,node_parent_ID_number,data_item1,data_item2,...,data_itemN\n

    The separator can, optionally, be a character other than ","

    The root node must have a parent id of 0 and normally should also have an index of 1

    From MATLAB one can produce tree structures and dump data in the correct format
    using https://github.com/raacampbell13/matlab-tree and the tree.dumptree method

    Inputs:
    fname - if a string, importData assumes it is a file name and tries to load the tree from file.
            if it is a list, importData assumes that each line is a CSV data line and tries to 
            convert to a tree.        
    displayTree - if True the tree is printed to standard output after creation
    colSep - the data separator, a comma by default.
    headerLine - if True, the first line is stripped off and considered to be the column headings.
                headerLine can also be a CSV string or a list that defines the column headings. Must have the
                same number of columns as the rest of the file.
    verbose - prints diagnositic info to screen if true
    """

    if verbose:
        print("tree.importData importing file %s" % fname)

    #Error check
    if isinstance(fname, str):
        if os.path.exists(fname) == False:
            print("Can not find file " + fname)
            return

        #Read in data
        fid = open(fname, 'r')
        contents = fid.read().split('\n')
        fid.close()

    elif isinstance(fname, list):
        contents = fname  #assume that fname is data rather than a file name

    #Get header data if present
    if headerLine == True:
        header = contents.pop(0)
        header = header.rstrip('\n').split(colSep)
    elif isinstance(headerLine, str):
        header = headerLine.rstrip('\n').split(colSep)
    elif isinstance(headerLine, list):
        header = headerLine
    else:
        header = False

    data = []
    for line in contents:
        if len(line) == 0:
            continue

        dataLine = line.split(colSep)
        if len(header) != len(dataLine):
            print(
                "\nTree file appears corrupt! header length is %d but data line length is %d.\ntree.importData is aborting.\n"
                % (len(header), len(dataLine)))
            return False

        theseData = list(
            map(int,
                dataLine[0:2]))  #add index and parent to the first two columns

        #Add data to the third column. Either as a list or as a dictionary (if header names were provided)
        if header != False:  #add as dictionary
            dataCol = dict()

            for ii in range(len(header) - 2):
                ii += 2
                dataCol[header[ii]] = dataTypeFromString.convertString(
                    dataLine[ii])

        else:
            dataCol = dataLine[2:]  #add as list of strings

        theseData.append(dataCol)
        data.append(theseData)

    if verbose:
        print("tree.importData read %d rows of data from %s" %
              (len(data), fname))

    #Build tree
    tree = Tree()
    tree.add_node(0)
    for thisNode in data:
        tree.add_node(thisNode[0], thisNode[1])
        tree[thisNode[0]].data = thisNode[2]

    #Optionally dump the tree to screen (unlikely to be useful for large trees)
    if displayTree:
        tree.display(0)

        for nodeID in tree.traverse(0):
            print("%s - %s" % (nodeID, tree[nodeID].data))

    return tree
예제 #25
0
if __name__ == "__main__":
    from running_vms import running_vms
    from running_groups import running_groups
    from vm_snaps import vm_snaps
    from tree import Tree
    
    rgs = running_groups()
    rg = rgs.pop()
    
    snaps_tree = Tree()
    snaps_tree.add_node(rg, None)
    
    for vm in running_vms():
        vm_snaps(vm, snaps_tree, rg)
    
    snaps_tree.display(rg)
예제 #26
0
    MLBD_all = []
    # record the chosen nodes with the lowest MLBD of each iteration
    nodes_all = []
    # record the time for each iteration
    time_iteration = []

    print "Start optimizing..."

    index = 0
    global num_node
    num_node = 0
    current_node = 0

    #Claim a tree
    tree = Tree()
    node = tree.add_node(current_node, theta_L, theta_U)
    num_node = num_node + 1

    start_all = time.clock()
    print x_all[-1]

    while index < MAXITER:
        start = time.clock()
        print "----------------------------iteration %d---------------------------" % index

        # Solve the subproblem
        objOpt, thetaOpt, lamOpt, muOpt = sub.solve_subproblem(
            y, xBar)  #(objOpt) upper bound

        thetaBar.append(thetaOpt)
        lamBar.append(lamOpt)
예제 #27
0
def run_ros():
	rospy.init_node('ros_demo')

	# First param: topic name; second param: type of the message to be published; third param: size of queued messages,
	# at least 1
	chatter_pub = rospy.Publisher('some_chatter', String, queue_size=10)
	marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
	path_points_pub = rospy.Publisher('path_points', PointArray, queue_size=10)

	# Each second, ros "spins" and draws 20 frames
	loop_rate = rospy.Rate(20)  # 20

	frame_count = 0

	obstacles = create_obstacles()

	obstacles_lines = get_obstacles_lines(obstacles)

	robot = create_robot()

	target = create_target()

	target_lines = get_target_lines(target)

	point = get_point_structure()

	tree_edges = get_tree_edges_structure()

	p0 = Point(robot.pose.position.x, robot.pose.position.y, 0)

	tree = Tree(TreeNode(p0, 0))

	collision_edges = get_collision_edges_structure()

	found_path = False

	path_edges = get_path_edges_structure()

	drawed_path = False

	robot_reached = False

	path_published = False

	path_points = []

	obstacle_collision_count = 0

	change_normal_distribution = False

	annoying_obstacle_point = None

	while not rospy.is_shutdown():
		msg = "Frame index: %s" % frame_count

		rospy.loginfo(msg)

		chatter_pub.publish(msg)

		for obst in obstacles:
			obst.header.stamp = rospy.Time.now()
			marker_pub.publish(obst)

		robot.header.stamp = rospy.Time.now()

		target.header.stamp = rospy.Time.now()
		marker_pub.publish(target)

		point.header.stamp = rospy.Time.now()

		tree_edges.header.stamp = rospy.Time.now()

		collision_edges.header.stamp = rospy.Time.now()

		path_edges.header.stamp = rospy.Time.now()

		if frame_count % HZ == 0 and not found_path:

			rand_pnt = None
			if change_normal_distribution:
				rand_pnt = choose_next_point(annoying_obstacle_pnt)
				change_normal_distribution = False
			else:
				rand_pnt = choose_next_point(target.pose.position)

			#enters this if statements every 5 counts
			# rand_pnt = choose_next_point(target)
			#rand_pnt = Point()
			#rand_pnt.x = random.uniform(BOARD_CORNERS[0], BOARD_CORNERS[1])
			#rand_pnt.y = random.uniform(BOARD_CORNERS[3], BOARD_CORNERS[2])
			#rand_pnt.z = 0
			point.points = [rand_pnt]
			close_node = tree.get_closest_node(rand_pnt)
			close_pnt = close_node.point

			# https://math.stackexchange.com/questions/175896/finding-a-point-along-a-line-a-certain-distance-away-from-another-point
			total_dist = math.sqrt(math.pow(rand_pnt.x - close_pnt.x, 2) + math.pow(rand_pnt.y - close_pnt.y, 2))

			dist_ratio = STEP / total_dist

			new_pnt = Point()
			new_pnt.x = (1 - dist_ratio) * close_pnt.x + dist_ratio * rand_pnt.x
			new_pnt.y = (1 - dist_ratio) * close_pnt.y + dist_ratio * rand_pnt.y
			new_pnt.z = 0

			does_collide, collision_point = collides_object(close_pnt, new_pnt, obstacles_lines)
			if does_collide:
				collision_edges.points.append(close_pnt)
				collision_edges.points.append(new_pnt)
				obstacle_collision_count += 1

				if obstacle_collision_count == 2:
					change_normal_distribution = True
					obstacle_collision_count = 0
					annoying_obstacle_pnt = collision_point

			else:
				# Try to find straight path by comparing the distance between new_pnt and all parents
				new_check_pnt_cost = 1 + close_node.cost_to_parent
				check_node = close_node.parent
				potential_parent = close_node
				potential_cost = 1
				while check_node != None:
					cost = math.sqrt(math.pow(check_node.point.x - new_pnt.x, 2) + math.pow(check_node.point.y - new_pnt.y, 2))
					if cost <= new_check_pnt_cost:
						if not collides_object(check_node.point, new_pnt, obstacles_lines)[0]:
							potential_cost = cost
							potential_parent = check_node
							new_check_pnt_cost = cost

					check_node = check_node.parent
					if check_node:
						new_check_pnt_cost += check_node.cost_to_parent


				close_node = potential_parent
				last_node = tree.add_node(close_node, new_pnt)

				tree_edges.points.append(close_node.point)
				tree_edges.points.append(new_pnt)

			if collides_object(close_node.point, new_pnt, target_lines)[0]:
				found_path = True

		if found_path and not drawed_path:
			current_node = last_node
			while not current_node.is_root():
				path_points.append(current_node.point)
				path_edges.points.append(current_node.point)
				path_edges.points.append(current_node.parent.point)
				current_node = current_node.parent
			drawed_path = True

		if found_path and drawed_path and not path_published:
			# path_poses = []

			# for i in range(len(path_points) - 1):
			# 	current_point = path_points[i]
			# 	next_point = path_points[i + 1]
			# 	current_pose = Pose()
			# 	current_pose.position = current_point
			# 	current_quat = Quaternion()
			# 	current_quat.x = 0
			# 	current_quat.y = 0
			# 	current_quat.z = 1

			# 	prev_angle = 0 
			# 	if i > 0:
			# 		prev_angle = path_poses[i - 1].orientation.w

			# 	current_quat.w = np.arctan((next_point.y -current_point.y)/ (next_point.x - current_point.x)) - prev_angle
			# 	current_pose.orientation = current_quat
			# 	path_poses.append(current_pose)

			# path_pose_array = PoseArray()
			# path_pose_array.poses = path_poses
			# path_points_pub.publish(path_pose_array)
			point_array = PointArray()
			point_array.points = path_points
			point_array.points.reverse()
			path_points_pub.publish(point_array)
			path_published = True

		
		if frame_count % 2 == 0 and drawed_path and not robot_reached:
			robot.pose.position = path_points.pop()
			robot_reached = True if len(path_points) == 0 else False

		if robot_reached: 
			break

		marker_pub.publish(robot)
		marker_pub.publish(point)
		marker_pub.publish(tree_edges)
		marker_pub.publish(collision_edges)
		marker_pub.publish(path_edges)



		# TODO: Robot function

		"""

		# From here, we are defining and drawing a simple robot

		rob.type = rob.SPHERE
		path.type = path.LINE_STRIP

		rob.header.frame_id = "map"
		path.header.frame_id = "map"

		rob.header.stamp = rospy.Time.now()
		path.header.stamp = rospy.Time.now()

		rob.ns = "rob"
		path.ns = "rob"

		rob.id = 0
		path.id = 1

		rob.action = rob.ADD
		path.action = path.ADD

		rob.lifetime = rospy.Duration()
		path.lifetime = rospy.Duration()

		rob.scale.x, rob.scale.y, rob.scale.z = 0.3, 0.3, 0.3

		rob.color.r, rob.color.g, rob.color.b, rob.color.a = 1.0, 0.5, 0.5, 1.0

		# Path line strip is blue
		path.color.b, path.color.a = 1.0, 1.0

		path.scale.x = 0.02
		path.pose.orientation.w = 1.0

		num_slice2 = 200 # Divide a circle into segments

		if frame_count % 2 == 0 and len(path.points) <= num_slice2:
			p = Point()

			angle = slice_index2 * 2 * math.pi / num_slice2
			slice_index2 += 1
			p.x = 4 * math.cos(angle) - 0.5 # Some random circular trajectory, with radius 4, and offset (-0.5, 1, .05)
			p.y = 4 * math.sin(angle) + 1.0
			p.z = 0.05

			rob.pose.position = p
			path.points.append(p) # For drawing path, which is line strip type

		marker_pub.publish(rob)
		marker_pub.publish(path)
		
		"""

		# To here, we finished displaying our components

		# Check if there is a subscriber. Here our subscriber will be Rviz
		while marker_pub.get_num_connections() < 1:
			if rospy.is_shutdown():
				return 0
			# rospy.logwarn_once("Please run Rviz in another terminal.")
			rospy.sleep(1)

		loop_rate.sleep()
		frame_count += 1
예제 #28
0
파일: planner.py 프로젝트: GPrathap/lqRRT
class Planner:
    """
    To initialize, provide...

    dynamics: Function that returns the next state given
              the current state x and the current effort u,
              and timestep dt. That is, xnext = dynamics(x, u, dt).

    lqr: Function that returns the local LQR cost-to-go matrix S
         and policy matrix K as a tuple of arrays (S, K) where S
         solves the local Riccati equation and K is the associated
         feedback gain matrix. That is, (S, K) = lqr(x, u).

    constraints: Instance of the Constraints class that defines
                 feasibility of states & efforts, goal region, etc...

    horizon: The simulation duration in seconds used to extend the tree. If you
             give a tuple (min, max), an adaptive horizon heuristic will be used.

    dt: The simulation timestep in seconds used to extend the tree.

    FPR: Failed Path Retention factor. When a path is grown and found to be infeasible,
         this is the fraction of the path that is retained up to that infeasible point.

    error_tol: The state error array or scalar defining controller convergence.

    erf: Function that takes two states xgoal and x and returns the state error
         between them. Defaults to simple subtraction xgoal - x. This is useful
         if your state includes a quaternion or heading.

    min_time: The least number of seconds that the tree will
              grow for. That is, even if a feasible plan is found
              before min_time, it will keep growing until min_time
              is reached and then give the best of the plans.

    max_time: The max number of seconds that the tree will grow for.
              That is, if there are still no feasible plans after this
              amount of time, the plan_reached_goal flag will remain False
              and the plan that gets closest to the goal is given.

    max_nodes: If the tree reaches this number of nodes but no path is
               found, the plan_reached_goal flag will remain False and the
               plan that gets closest to the goal is given.

    goal0: The initial goal state. If left as None, update_plan
           cannot be run. Use set_goal to set the goal at any time.
           Be sure to update the plan after setting a new goal.

    sys_time: Function that returns the real-world system time.
              Defaults to the Python time library's time().

    printing: Bool that specifies if internal stuff should be printed.

    """
    def __init__(self,
                 dynamics,
                 lqr,
                 constraints,
                 horizon,
                 dt=0.05,
                 FPR=0,
                 error_tol=0.05,
                 erf=np.subtract,
                 min_time=0.5,
                 max_time=1,
                 max_nodes=1E5,
                 goal0=None,
                 sys_time=time.time,
                 printing=True):

        self.set_system(dynamics, lqr, constraints, erf)

        self.set_resolution(horizon, dt, FPR, error_tol)

        self.set_runtime(min_time, max_time, max_nodes, sys_time)

        self.set_goal(goal0)

        self.printing = printing
        self.killed = False

#################################################

    def update_plan(self,
                    x0,
                    sample_space,
                    goal_bias=0,
                    guide=None,
                    xrand_gen=None,
                    pruning=True,
                    finish_on_goal=False,
                    specific_time=None):
        """
        A new tree is grown from the seed x0 in an attempt to plan
        a path to the goal. The returned path can be accessed with
        the interpolator functions get_state(t) and get_effort(t).

        The tree is motivated by uniform random samples in the over
        the given sample_space. The sample_space is a list of n tuples
        where n is the number of states; [(min1, max1), (min2, max2)...].

        The goal_bias is the fraction of the time the goal is sampled.
        It can be a scalar from 0 (none of the time) to 1 (all the time)
        or a list of scalars corresponding to each state dimension.

        Alternatively, you can give a function xrand_gen which takes the
        current planner instance (self) and outputs the random sample state.
        Doing this will override both sample_space and goal_bias, which you
        can set to arbitrary values only if you provide an xrand_gen function.

        Or, instead of a function, you can set xrand_gen to a single integer
        1 or greater, which will act as the number of tries allowed for
        finding a feasible random sample in the default random sampling
        routine. (Leaving the default None will set the limit to 10 tries).

        After min_time seconds, the fastest available path from x0 to
        the current goal is returned and the functions get_state(t)
        and get_effort(t) are modified to interpolate this new path.

        If no path was found yet, the search continues until max_time or
        until the node limit is breached. After the limit, a warning is
        printed and the path that gets nearest to the guide is
        used instead. If guide is left None, it defaults to goal.

        If pruning is True, then nodes can be marked as "ignore" during
        growth. Right now, only nodes on a completed path are ignored.

        If finish_on_goal is set to True, once the plan makes it to the goal
        region (goal plus buffer), it will attempt to steer one more path
        directly into the exact goal. Can fail for nonholonomic systems.

        If you want this update_plan to plan for some specific amount of
        time (instead of using the global min_time and max_time), pass it
        in as specific_time in seconds.

        This function returns True if it finished fully, or False if it was
        haulted. It can hault if it is killed or if the tree exceeds max_nodes,
        or if no goal has been set yet.

        """
        # Safety first!
        x0 = np.array(x0, dtype=np.float64)
        if self.goal is None:
            print("No goal has been set yet!")
            self.get_state = lambda t: x0
            self.get_effort = lambda t: np.zeros(self.ncontrols)
            return False

        # Store timing
        if specific_time is None:
            min_time = self.min_time
            max_time = self.max_time
        else:
            min_time = specific_time
            max_time = specific_time

        # Reset the tree
        self.tree = Tree(x0, self.lqr(x0, np.zeros(self.ncontrols)))
        ignores = np.array([])

        # If not given an xrand_gen function, make the standard one
        if xrand_gen is None or type(xrand_gen) is int:

            # Properly cast the given goal bias
            if goal_bias is None:
                goal_bias = [0] * self.nstates
            elif hasattr(goal_bias, '__contains__'):
                if len(goal_bias) != self.nstates:
                    raise ValueError(
                        "Expected goal_bias to be scalar or have same length as state."
                    )
            else:
                goal_bias = [goal_bias] * self.nstates

            # Set the number of tries for sample feasibility
            if xrand_gen > 0:
                tries_limit = xrand_gen
            else:
                tries_limit = 10

            # Properly cast the given sample space and extract statistics
            sample_space = np.array(sample_space, dtype=np.float64)
            if sample_space.shape != (self.nstates, 2):
                raise ValueError(
                    "Expected sample_space to be list of nstates tuples.")
            sampling_centers = np.mean(sample_space, axis=1)
            sampling_spans = np.diff(sample_space).flatten()

            # Standard sampling
            def xrand_gen(planner):
                tries = 0
                while tries < tries_limit:
                    xrand = sampling_centers + sampling_spans * (
                        np.random.sample(self.nstates) - 0.5)
                    for i, choice in enumerate(
                            np.greater(goal_bias, np.random.sample())):
                        if choice:
                            xrand[i] = self.goal[i]
                    if self.constraints.is_feasible(xrand,
                                                    np.zeros(self.ncontrols)):
                        return xrand
                    tries += 1
                return xrand

        # Otherwise, use given sampling function
        else:
            if not hasattr(xrand_gen, '__call__'):
                raise ValueError(
                    "Expected xrand_gen to be None, an integer >= 1,  or a function."
                )

        # Store guide state
        if guide is None:
            self.xguide = np.copy(self.goal)
        else:
            self.xguide = np.array(guide, dtype=np.float64)

        # Loop managers
        if self.printing:
            print("\n...planning...")
        self.plan_reached_goal = False
        self.T = np.inf
        time_elapsed = 0
        time_start = self.sys_time()

        # Planning loop!
        while True:
            # Random sample state
            xrand = xrand_gen(self)
            print(xrand)
            # The "nearest" node to xrand has the least cost-to-go of all nodes
            if pruning:
                nearestIDs = np.argsort(self._costs_to_go(xrand))
                nearestID = nearestIDs[0]
                for ID in nearestIDs:
                    if ID not in ignores:
                        nearestID = ID
                        break
            else:
                nearestID = np.argmin(self._costs_to_go(xrand))

            print(nearestID)

            # Candidate extension to the tree
            xnew_seq, unew_seq = self._steer(nearestID,
                                             xrand,
                                             force_arrive=False)

            # If steer produced any feasible results, extend tree
            if len(xnew_seq) > 0:

                # Add the new node to the tree
                xnew = np.copy(xnew_seq[-1])
                self.tree.add_node(nearestID, xnew,
                                   self.lqr(xnew, np.copy(unew_seq[-1])),
                                   xnew_seq, unew_seq)

                # Check if the newest node reached the goal region
                if self._in_goal(xnew):

                    # Raise flag
                    self.plan_reached_goal = True

                    # Climb tree to construct sequence of states for this path
                    node_seq = self.tree.climb(self.tree.size - 1)
                    x_seq, u_seq = self.tree.trajectory(node_seq)

                    # Ignore nodes on any succeeded path
                    ignores = np.unique(np.concatenate((ignores, node_seq)))

                    # Expected time to complete this plan
                    T = len(x_seq) * self.dt

                    # Retain this plan if it is faster than the previous one
                    if T < self.T:
                        self.T = T
                        self.node_seq = node_seq
                        self.x_seq = x_seq
                        self.u_seq = u_seq
                        self.t_seq = np.arange(len(self.x_seq)) * self.dt
                        if self.printing:
                            print("Found plan at elapsed time: {} s".format(
                                np.round(time_elapsed, 6)))

            # For checking if we should stop planning
            time_elapsed = self.sys_time() - time_start

            # Abrupt termination
            if self.killed:
                break

            # Close-out for reached-goal
            elif self.plan_reached_goal and time_elapsed >= min_time:
                if finish_on_goal:
                    # Steer to exact goal

                    xgoal_seq, ugoal_seq = self._steer(self.node_seq[-1],
                                                       self.goal,
                                                       force_arrive=True)
                    # If it works, tack it onto the plan
                    if len(xgoal_seq) > 0:
                        self.tree.add_node(self.node_seq[-1], self.goal, None,
                                           xgoal_seq, ugoal_seq)
                        self.node_seq.append(self.tree.size - 1)
                        self.x_seq.extend(xgoal_seq)
                        self.u_seq.extend(ugoal_seq)
                        self.t_seq = np.arange(len(self.x_seq)) * self.dt
                # Over and out!
                if self.printing:
                    print("Tree size: {0}\nETA: {1} s".format(
                        self.tree.size, np.round(self.T, 2)))
                self._prepare_interpolators()
                break

            # Close-out for didn't-reach-goal
            elif time_elapsed >= max_time or self.tree.size > self.max_nodes:
                # Find closest node to guide state
                Sguide = self.lqr(self.xguide, np.zeros(self.ncontrols))[0]
                for i, g in enumerate(self.constraints.goal_buffer):
                    if np.isinf(g):
                        Sguide[:, i] = 0
                guide_diffs = self.erf_v(self.xguide, self.tree.state)
                closestID = np.argmin(
                    np.sum(np.tensordot(guide_diffs, Sguide, axes=1) *
                           guide_diffs,
                           axis=1))
                self.node_seq = self.tree.climb(closestID)
                # Construct plan
                self.x_seq, self.u_seq = self.tree.trajectory(self.node_seq)
                self.T = len(self.x_seq) * self.dt
                self.t_seq = np.arange(len(self.x_seq)) * self.dt
                # Over and out!
                if self.printing:
                    print("Didn't reach goal.\nTree size: {0}\nETA: {1} s".
                          format(self.tree.size, np.round(self.T, 2)))
                self._prepare_interpolators()
                break

        if self.killed or self.tree.size > self.max_nodes:
            if self.printing:
                print("Plan update terminated abruptly!")
            self.killed = False
            return False
        else:
            return True

#################################################

    def _costs_to_go(self, x):
        """
        Returns an array of costs to go to x for each node in the
        current tree, in the same ordering as the nodes. This cost
        is  (v-x).T * S * (v-x)  for each node state v where S is
        found by LQR about x, not v.

        """
        S = self.lqr(x, np.zeros(self.ncontrols))[0]
        # print(self.tree.state)
        diffs = self.erf_v(x, self.tree.state)
        return np.sum(np.tensordot(diffs, S, axes=1) * diffs, axis=1)

#################################################

    def _steer(self,
               ID,
               xtar,
               force_arrive=False
               ):  #<<< need to numpy this function for final speedup!
        """
        Starting from the given node ID's state, the system dynamics are
        forward simulated using the local LQR policy toward xtar.

        If the state updates into an infeasible condition, the simulation
        is finished and the path returned is half what was generated.

        If force_arrive is set to True, then the simulation isn't finished
        until xtar is achieved or until a physical timeout.

        If it is False, then the simulation will stop after self.horizon sim
        seconds or if the error drops below some reasonable self.error_tol.

        Returns the sequences of states and efforts. Note that the initial
        state is not included in the returned trajectory (to avoid tree overlap).
        """
        # Set up
        K = np.copy(self.tree.lqr[ID][1])
        x = np.copy(self.tree.state[ID])
        x_seq = []
        u_seq = []
        last_emag = np.inf
        # Management
        i = 0
        elapsed_time = 0
        start_time = self.sys_time()
        # Simulate
        while True:
            # Compute effort using local LQR policy
            e = self.erf(np.copy(xtar), np.copy(x))
            # dy(e)
            u = K.dot(e)
            # Step forward dynamics
            x = self.dynamics(np.copy(x), np.copy(u), self.dt)
            # Check for feasibility
            if not self.constraints.is_feasible(x, u):
                x_seq = x_seq[:int(self.FPR * len(x_seq))]
                u_seq = u_seq[:int(self.FPR * len(u_seq))]
                break
            # Check force-arrive finish criteria
            if force_arrive:
                # Physical time limit
                elapsed_time = self.sys_time() - start_time
                if elapsed_time > np.clip(self.min_time / 2, 0.1, np.inf):
                    if self.printing:
                        print("(exact goal-convergence timed-out)")
                    break
                # Definite convergence criteria
                if np.allclose(x, xtar, rtol=1E-4, atol=1E-4):
                    break
            # or check lenient-arrive finish criteria
            else:
                i += 1
                emag = np.abs(e)
                # Adaptive horizon heuristic
                if self.hfactor:
                    if np.all(emag >= last_emag):
                        x_seq = []
                        u_seq = []
                        self.horizon_iters = int(
                            np.clip(self.horizon_iters / self.hfactor,
                                    self.hspan[0], self.hspan[1]))
                        break
                    if i == self.horizon_iters:
                        self.horizon_iters = int(
                            np.clip(self.hfactor * self.horizon_iters,
                                    self.hspan[0], self.hspan[1]))
                    last_emag = emag
                # Horizon, or tolerable convergence criteria
                if i > self.horizon_iters or np.all(emag <= self.error_tol):
                    break
            # Record
            x_seq.append(x)
            u_seq.append(u)
            # Get next control policy
            K = self.lqr(x, u)[1]
        return (x_seq, u_seq)

#################################################

    def _in_goal(self, x):
        """
        Returns True if some state x is in the goal region.
        """
        return all(goal_span[0] < v < goal_span[1]
                   for goal_span, v in zip(self.goal_region, x))

#################################################

    def _prepare_interpolators(self):
        """
        Updates the interpolator functions the user calls
        to interpolate the current plan.

        """
        if len(self.x_seq) == 1:
            self.get_state = lambda t: self.x_seq[0]
            self.get_effort = lambda t: np.zeros(self.ncontrols)
        else:
            self.get_state = interp1d(self.t_seq,
                                      np.array(self.x_seq),
                                      axis=0,
                                      assume_sorted=True,
                                      bounds_error=False,
                                      fill_value=self.x_seq[-1][:])
            self.get_effort = interp1d(self.t_seq,
                                       np.array(self.u_seq),
                                       axis=0,
                                       assume_sorted=True,
                                       bounds_error=False,
                                       fill_value=self.u_seq[-1][:])

#################################################

    def set_goal(self, goal):
        """
        Modifies the goal state and region.
        Be sure to update the plan after modifying the goal.

        """
        if goal is None:
            self.goal = None
        else:
            if len(goal) == self.nstates:
                self.goal = np.array(goal, dtype=np.float64)
            else:
                raise ValueError(
                    "The goal state must have same dimensionality as state space."
                )

            goal_region = []
            for i, buff in enumerate(self.constraints.goal_buffer):
                goal_region.append((self.goal[i] - buff, self.goal[i] + buff))

            self.goal_region = goal_region
            self.plan_reached_goal = False

#################################################

    def set_runtime(self,
                    min_time=None,
                    max_time=None,
                    max_nodes=None,
                    sys_time=None):
        """
        See class docstring for argument definitions.
        Arguments not given are not modified.

        """
        if min_time is not None:
            self.min_time = min_time

        if max_time is not None:
            self.max_time = max_time

        if self.min_time > self.max_time:
            raise ValueError(
                "The min_time must be less than or equal to the max_time.")

        if max_nodes is not None:
            self.max_nodes = max_nodes

        if sys_time is not None:
            if hasattr(sys_time, '__call__'):
                self.sys_time = sys_time
            else:
                raise ValueError("Expected sys_time to be a function.")

#################################################

    def set_resolution(self, horizon=None, dt=None, FPR=None, error_tol=None):
        """
        See class docstring for argument definitions.
        Arguments not given are not modified.

        """
        if horizon is not None:
            self.horizon = horizon

        if dt is not None:
            self.dt = dt

        if FPR is not None:
            self.FPR = FPR

        if error_tol is not None:
            if np.shape(error_tol) in [(), (self.nstates, )]:
                self.error_tol = np.abs(error_tol).astype(np.float64)
            else:
                raise ValueError(
                    "Shape of error_tol must be scalar or length of state.")

        if hasattr(self.horizon, '__contains__'):
            if len(self.horizon) != 2:
                raise ValueError(
                    "Expected horizon to be tuple (min, max) or a single scalar."
                )
            if self.horizon[0] < self.dt:
                raise ValueError(
                    "The minimum horizon must be at least as big as dt.")
            if self.horizon[0] >= self.horizon[1]:
                raise ValueError(
                    "A horizon range tuple must be given as (min, max) where min < max."
                )
            self.horizon_iters = 1
            self.hspan = np.divide(self.horizon, self.dt).astype(np.int64)
            self.hfactor = int(2)
        elif self.horizon >= self.dt:
            self.horizon_iters = int(self.horizon / self.dt)
            self.hspan = (self.horizon_iters, self.horizon_iters)
            self.hfactor = 0
        else:
            raise ValueError("The horizon must be at least as big as dt.")

#################################################

    def set_system(self, dynamics=None, lqr=None, constraints=None, erf=None):
        """
        See class docstring for argument definitions.
        Arguments not given are not modified.
        If dynamics gets modified, so must lqr (and vis versa).

        """
        if dynamics is not None or lqr is not None:
            if hasattr(dynamics, '__call__'):
                self.dynamics = dynamics
            else:
                raise ValueError("Expected dynamics to be a function.")
            if hasattr(lqr, '__call__'):
                self.lqr = lqr
            else:
                raise ValueError("Expected lqr to be a function.")

        if constraints is not None:
            if isinstance(constraints, Constraints):
                self.constraints = constraints
                self.nstates = self.constraints.nstates
                self.ncontrols = self.constraints.ncontrols
            else:
                raise ValueError(
                    "Expected constraints to be an instance of the Constraints class."
                )

        if erf is not None:
            if hasattr(erf, '__call__'):
                self.erf = erf
                if erf is np.subtract:
                    self.erf_v = erf
                else:
                    self.erf_v = lambda g, x: -np.apply_along_axis(
                        erf, 1, x, g)
            else:
                raise ValueError("Expected erf to be a function.")

        self.plan_reached_goal = False

#################################################

    def kill_update(self):
        """
        Raises a flag that will cause an abrupt termination of the update_plan routine.

        """
        self.killed = True

#################################################

    def unkill(self):
        """
        Lowers the kill_update flag. Do this if you made a mistake.

        """
        self.killed = False

#################################################

    def visualize(self, dx, dy):
        """
        Plots the (dx,dy)-cross-section of the current tree,
        and highlights the current plan's trajectory.
        For example, dx=0, dy=1 plots the states #0 and #1.

        """
        if hasattr(self, 'node_seq'):
            self.tree.visualize(dx, dy, node_seq=self.node_seq)
        else:
            print("There is no plan to visualize!")
예제 #29
0
from tree import Tree
import myconst

tree = Tree()

tree.add_node("Task")
tree.add_node("B1", "Task")
tree.add_node("B2", "Task")
tree.add_node("B3", "Task")
tree.add_node("B4", "B1")
tree.add_node("B5", "B2")
tree.add_node("L1", "B5")
tree.add_node("B6", "B2")
tree.add_node("L2", "B6")

tree.display("Task")
'''
tree.remove_node("B5",myconst.YES)
tree.add_node("B5","B2")
tree.add_node("L1","B5")
tree.display("Task")
tree.remove_node("B5",myconst.NO)
tree.display("Task")
tree.display_dict()

print("***** DEPTH-FIRST ITERATION *****")
for node in tree.traverse("Task"):
    print(node)
print("***** BREADTH-FIRST ITERATION *****")
for node in tree.traverse("Task", mode=BREADTH):
    print(node)