def extend_RRT_MILP(s, T, eps, K, x_sample_list):
i = choice(s.modes)
#x_sample=sample(s.l[i],s.u[i])
if x_sample_list == []:
x_sample = sample_from_mode(s, i)
else:
x_sample = x_sample_list[0]
array_tree(s)
if inside_tree(s, x_sample) == True:
print("inside X_tree")
return False
else:
(x_zero, T) = simulate_0(s, x_sample, T)
STATES = sorted_distance_states(s, x_zero)
# using list comprehension
chunck_list = [
STATES[i * K:(i + 1) * K]
for i in range((len(STATES) + K - 1) // K)
]
for list_of_states in chunck_list:
(x, u, G, theta, z, flag,
state_end) = polytopic_trajectory_to_set_of_polytopes(
s, x_sample, T, list_of_states, eps, method="chull")
if flag == True:
if all_vertices_out_of_tree(s, x[0], G[0]) == True:
make_state_trajectory_state_end(s, x, u, z, G, theta, T,
state_end)
print("+" * 80, "Connected to Tree, size=%d" % len(s.X),
"+" * 80)
return True
def intitialize_tree(s, T=20, alpha_start=0):
goal = s.goal
s.goal.successor = (s.goal, "null", "null")
x0 = s.goal.x
(x, u, G, theta, z, flag) = polytope_trajectory(s,
x0,
goal,
T,
alpha_start,
coin=0.5)
if flag == True:
print("Initilization Successfull!")
make_state_trajectory_state_end(s, x, u, z, G, theta, T, goal)
s.X.append(s.goal)
array_tree(s)
s.tree_iterations += 1
s.tree_size[s.tree_iterations] = len(s.X)
else:
raise ("Error! Initilization NOT successfull!")
def extend_RRT(s, T, alpha_start=10**5, eps=0.1):
i = choice(s.modes)
x_sample = sample(s.l[i], s.u[i])
array_tree(s)
if inside_tree(s, x_sample) == True:
print("inside X_tree")
return False
else:
(x_zero, T) = simulate_0(s, x_sample, T)
STATES = sorted_distance_states(s, x_zero)
for state_end in STATES:
(x, u, G, theta, z,
flag) = polytope_trajectory(s, x_sample, state_end, T,
alpha_start, eps)
if flag == True:
if all_vertices_out_of_tree(s, x[0], G[0]) == True:
make_state_trajectory_state_end(s, x, u, z, G, theta, T,
state_end)
print("+" * 80, "Connected to Tree", "+" * 80)
return True
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Aug 14 18:39:56 2018
@author: sadra
"""
### Internal imports
import sys
sys.path.append('../..')
from main.trajectory import state_trajectory
from main.tree_locator import inside_tree, array_tree
from main.auxilary_methods import sample
from main.visualization import visualize_subset_tree, visualize_set_tube_simulation
from main.simulate import simulate_vanilla, simulate_convex
array_tree(s)
x0 = np.array([0.05, 0.5]).reshape(2, 1)
simulate_convex(s, x0)
visualize_set_tube_simulation(s, -0.12, 0.12, -1, 1, "Angle",
"Angular Velocity", "time_optimal")