def distance_from_goal(node):
return 0
def goal_test(node):
goal_region_radius = .01
n = 4
return np.sum(np.abs(node['state'][0:n] -
goal[0:n])) < goal_region_radius #disregards time
return distance(
node, goal
) < goal_region_radius #need to think more carefully about this one
start = np.array([0, 0, 0, 0, 0])
lqr_rrt = LQR_RRT(A, B, Q, R, max_time_horizon)
rrt = RRT(state_ndim=5, control_ndim=2)
lqr_rrt.action_state_valid = action_state_valid
lqr_rrt.max_nodes_per_extension = 5
rrt.sample_goal = lambda: goal
rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
def goal_test(node):
goal_region_radius = .01
n = 2
return np.sum(np.abs(node['state'][0:n] -
goal[0:n])) < goal_region_radius #disregards time
return distance(
node, goal
) < goal_region_radius #need to think more carefully about this one
def distance_from_goal(node):
return 0
lqr_rrt = LQR_RRT(A, B, Q, R, max_time_horizon)
lqr_rrt.action_state_valid = action_state_valid
def sample():
if np.random.rand() < .9:
statespace = np.random.rand(2) * np.array([.4, 2]) - np.array([.2, 1])
time = np.random.randint(0, max_time_horizon, size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time, newshape=(1, ))
return np.concatenate((statespace, time))
else: #goal bias
statespace = goal[0:2]
time = np.random.randint(0, max_time_horizon, size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time, newshape=(1, ))
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time,newshape=(1,))
return np.concatenate((statespace,time))
def distance_from_goal(node):
return 0
def goal_test(node):
goal_region_radius = .01
n = 4
return np.sum(np.abs(node['state'][0:n]-goal[0:n])) < goal_region_radius #disregards time
return distance(node,goal) < goal_region_radius #need to think more carefully about this one
start = np.array([0,0,0,0,0])
lqr_rrt = LQR_RRT(A,B,Q,R,max_time_horizon)
rrt = RRT(state_ndim=5,control_ndim=2)
lqr_rrt.action_state_valid = action_state_valid
lqr_rrt.max_nodes_per_extension = 5
rrt.sample_goal = lambda : goal
rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
return r
def action_state_valid(x,u):
return isStateValid(x) and isActionValid(u)
def goal_test(node):
goal_region_radius = .01
n = 2
return np.sum(np.abs(node['state'][0:n]-goal[0:n])) < goal_region_radius #disregards time
return distance(node,goal) < goal_region_radius #need to think more carefully about this one
def distance_from_goal(node):
return 0
lqr_rrt = LQR_RRT(A,B,Q,R,max_time_horizon)
lqr_rrt.action_state_valid = action_state_valid
def sample():
if np.random.rand()<.9:
statespace = np.random.rand(2)*np.array([.4,2])-np.array([.2,1])
time = np.random.randint(0,max_time_horizon,size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time,newshape=(1,))
return np.concatenate((statespace,time))
else: #goal bias
statespace = goal[0:2]
time = np.random.randint(0,max_time_horizon,size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time,newshape=(1,))
return np.concatenate((statespace,time))
