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)
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)
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)
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 = []
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)
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
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
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
#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")
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()
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)
(_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)
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()
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("")
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
## 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)
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
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())
# 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)
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)
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
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
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)
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)
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
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!")
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)
