pathy = []
for state in motion_primitives[i].path:
pathx.append(state[0])
pathy.append(state[1])
plt.plot(pathx, pathy)
plt.show(block=False)
def cost_function(state, motion_primitive):
return motion_primitive.cost # + belief cost
astar = AStar(motion_primitives, cost_function, heuristic, valid_edge_function,
state_equality)
plan = astar.plan(start_state, goal_state)
if plan:
pathx = []
pathy = []
for i in range(len(plan) - 1, 0, -1):
seg, _ = dubins.path_sample(plan[i], plan[i - 1], turning_radius,
step_size)
for state in seg:
pathx.append(state[0])
pathy.append(state[1])
print state
plt.figure(2)
plt.subplot(111)
temp_idx = dub.last_idx
while following_dist < dub.look_ahead_dist
temp_idx -= 1
path_diff = numpy.array([,])
following_dist += np.linalg.norm(path_diff)
index = temp_idx
'''
#ind_diff = np.abs(np.array(indices) - carrot_idx)
#index = indices[np.argmin(ind_diff)]
astar_state = np.array(
[state['x'], state['y'],
state['theta']]) #path_states[index,:]
invalidPath = False
astar_goal = np.array([goal[0], goal[1], 0])
plan, cost = astar.plan(astar_state, astar_goal)
if plan is not None:
print 'path cost:'
print cost
path_states = motion_primitive.get_xytheta_paths(plan)
dub.last_seg = 1 # not 0 since 0 is the root and has not path segment
dub.last_idx = 0
astar_failed = False
print 'done'
else:
astar_failed = True
break
#compute action
action = dub.control_policy(state, plan)
self.delta_state = delta_state
def get_end_state(self, state):
return state + self.delta_state
delta_states = [np.array([ 1, 0]),
np.array([ 1, 1]),
np.array([ 0, 1]),
np.array([-1, 1]),
np.array([-1, 0]),
np.array([-1, -1]),
np.array([ 0, -1]),
np.array([ 1, -1])]
motion_primitives = [motion_primitive(delta_state) for delta_state in delta_states]
def cost_function(state, motion_primitive):
return np.linalg.norm(motion_primitive.delta_state)
astar = AStar(motion_primitives, cost_function, heuristic, valid_edge_function, state_equality)
plan = astar.plan(start_state, goal_state)
pathx = [state[0] for state in plan]
pathy = [state[1] for state in plan]
plt.plot(pathx,pathy)
plt.plot([0, 7],[3, 3])
plt.plot([3, 10],[6, 6])
plt.show()
'''
following_dist = 0.0
temp_idx = dub.last_idx
while following_dist < dub.look_ahead_dist
temp_idx -= 1
path_diff = numpy.array([,])
following_dist += np.linalg.norm(path_diff)
index = temp_idx
'''
#ind_diff = np.abs(np.array(indices) - carrot_idx)
#index = indices[np.argmin(ind_diff)]
astar_state = np.array([state['x'],state['y'],state['theta']]) #path_states[index,:]
invalidPath = False
astar_goal = np.array([goal[0],goal[1],0])
plan, cost = astar.plan(astar_state, astar_goal)
if plan is not None:
print 'path cost:'
print cost
path_states = motion_primitive.get_xytheta_paths(plan)
dub.last_seg = 1 # not 0 since 0 is the root and has not path segment
dub.last_idx = 0
astar_failed = False
print 'done'
else:
astar_failed = True
break
#compute action
action = dub.control_policy(state, plan)
