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)