def test_py_system_sst_custom_distance(): ''' Check that distance overriding in python works ''' system = Acrobot() planner = _sst_module.SSTWrapper( state_bounds=system.get_state_bounds(), control_bounds=system.get_control_bounds(), # use custom distance computer distance=AcrobotDistance(), start_state=np.array([0., 0., 0., 0.]), goal_state=np.array([np.pi, 0., 0., 0.]), goal_radius=2., random_seed=0, sst_delta_near=0.6, sst_delta_drain=0.4) min_time_steps = 10 max_time_steps = 50 integration_step = 0.02 for iteration in range(100): planner.step(system, min_time_steps, max_time_steps, integration_step) im = planner.visualize_tree(system)
def neural_replanner(mpNet1, mpNet2, start_node, goal_node, real_goal_node, obc, obs, IsInCollisionWithObs, normalize, unnormalize, MAX_LENGTH, step_sz, num_steps, informer, init_informer, system, dynamics, enforce_bounds, traj_opt, data): # visualization print('SHEN ME GUI') print('step_sz: %f' % (step_sz)) params = {} params['obs_w'] = 6. params['obs_h'] = 6. params['integration_step'] = step_sz vis = AcrobotVisualizer(Acrobot(), params) vis.obs = obs plt.ion() fig = plt.figure() ax = fig.add_subplot(121) ax.set_autoscale_on(True) hl, = ax.plot([], [], 'b') #hl_real, = ax.plot([], [], 'r') hl_for, = ax.plot([], [], 'g') hl_back, = ax.plot([], [], 'r') hl_for_mpnet, = ax.plot([], [], 'lightgreen') hl_back_mpnet, = ax.plot([], [], 'salmon') ax_ani = fig.add_subplot(122) vis._init(ax_ani) print(obs) def update_line(h, ax, new_data): new_data = wrap_angle(new_data, system) h.set_data(np.append(h.get_xdata(), new_data[0]), np.append(h.get_ydata(), new_data[1])) #h.set_xdata(np.append(h.get_xdata(), new_data[0])) #h.set_ydata(np.append(h.get_ydata(), new_data[1])) def remove_last_k(h, ax, k): h.set_data(h.get_xdata()[:-k], h.get_ydata()[:-k]) def draw_update_line(ax): ax.relim() ax.autoscale_view() fig.canvas.draw() fig.canvas.flush_events() #plt.show() def animation(states, actions): vis._animate(states[-1], ax_ani) draw_update_line(ax_ani) dtheta = 0.1 feasible_points = [] infeasible_points = [] goal_region = [] imin = 0 imax = int(2*np.pi/dtheta) obs_width = 6. new_obs_i = [] for k in range(len(obs)): obs_pt = [] obs_pt.append(obs[k][0]-obs_width/2) obs_pt.append(obs[k][1]-obs_width/2) obs_pt.append(obs[k][0]-obs_width/2) obs_pt.append(obs[k][1]+obs_width/2) obs_pt.append(obs[k][0]+obs_width/2) obs_pt.append(obs[k][1]+obs_width/2) obs_pt.append(obs[k][0]+obs_width/2) obs_pt.append(obs[k][1]-obs_width/2) new_obs_i.append(obs_pt) IsInCollision = lambda x: IsInCollisionWithObs(x, new_obs_i) for i in range(imin, imax): for j in range(imin, imax): x = np.array([dtheta*i-np.pi, dtheta*j-np.pi, 0., 0.]) if IsInCollision(x): infeasible_points.append(x) else: feasible_points.append(x) if goal_check(Node(x), real_goal_node): goal_region.append(x) feasible_points = np.array(feasible_points) infeasible_points = np.array(infeasible_points) goal_region = np.array(goal_region) ax.scatter(feasible_points[:,0], feasible_points[:,1], c='yellow') ax.scatter(infeasible_points[:,0], infeasible_points[:,1], c='pink') ax.scatter(goal_region[:,0], goal_region[:,1], c='green') for i in range(len(data)): update_line(hl, ax, data[i]) draw_update_line(ax) update_line(hl_for, ax, start_node.x) draw_update_line(ax) update_line(hl_back, ax, goal_node.x) draw_update_line(ax) # plan a mini path from start to goal # obs: tensor start = start_node.x goal = goal_node.x itr=0 pA=[] pA.append(start) pB=[] pB.append(goal) target_reached=0 tree=0 new_path = [] # extract the state for the node start = torch.from_numpy(start_node.x).type(torch.FloatTensor) goal = torch.from_numpy(goal_node.x).type(torch.FloatTensor) start_numpy = start.numpy() goal_numpy = goal.numpy() x_init, u_init, t_init = init_informer(obs, Node(start_numpy), Node(goal_numpy), direction=0) goal_numpy__node, edge, cf = pathSteerTo(Node(start_numpy), Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=False, endpoint=True) if not node_nearby(edge.xs[0], start_numpy, np.identity(len(start_numpy)), 1e-2, system): goal_numpy__node, edge, cf = pathSteerTo(Node(start_numpy), Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=True, endpoint=True) target_reached=cf and node_nearby(goal_numpy__node.x, goal_numpy, np.diag([1.,1.,0.5,0.5]), np.sqrt(1.), system) if goal_check(Node(goal_numpy), real_goal_node): target_reached = target_reached and goal_check(goal_numpy__node, real_goal_node) #--- compute MPNet waypoints connecting start and goal while target_reached==0 and itr<MAX_LENGTH*4: itr=itr+1 # prevent the path from being too long if tree==0: ip1 = torch.cat((start, goal)).unsqueeze(0) ob1 = torch.FloatTensor(obc).unsqueeze(0) #ip1=torch.cat((obs,start,goal)).unsqueeze(0) time0 = time.time() ip1=normalize(ip1) ip1=to_var(ip1) ob1=to_var(ob1) waypoint=mpNet1(ip1,ob1).squeeze(0) # unnormalize to world size waypoint=waypoint.data.cpu() time0 = time.time() waypoint = unnormalize(waypoint).numpy() waypoint = wrap_angle(waypoint, system) waypoint = torch.from_numpy(waypoint).type(torch.FloatTensor) if IsInCollision(waypoint.numpy()): tree=1 continue start = waypoint pA.append(start.numpy()) tree=1 else: #tree=0 #continue ip2 = torch.cat((goal, start)).unsqueeze(0) ob2 = torch.FloatTensor(obc).unsqueeze(0) #ip2=torch.cat((obs,goal,start)).unsqueeze(0) time0 = time.time() ip2=normalize(ip2) ip2=to_var(ip2) ob2=to_var(ob2) waypoint=mpNet2(ip2,ob2).squeeze(0) # unnormalize to world size waypoint=waypoint.data.cpu() time0 = time.time() waypoint = unnormalize(waypoint).numpy() waypoint = wrap_angle(waypoint, system) waypoint = torch.from_numpy(waypoint).type(torch.FloatTensor) if IsInCollision(waypoint.numpy()): tree=0 continue goal = waypoint pB.append(goal.numpy()) tree=0 #target_reached=MPNetSteerTo(start, goal, ) #target_reached=node_nearby(start.numpy(), goal.numpy(), np.diag([1.,1.,0.1,0.1]), 1., system) # try to use trajopt and then test if endpoints can connect start_numpy = start.numpy() goal_numpy = goal.numpy() x_init, u_init, t_init = init_informer(obs, Node(start_numpy), Node(goal_numpy), direction=0) goal_numpy__node, edge, cf = pathSteerTo(Node(start_numpy), Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=False, endpoint=True) if not node_nearby(edge.xs[0], start_numpy, np.identity(len(start_numpy)), 1e-2, system): goal_numpy__node, edge, cf = pathSteerTo(Node(start_numpy), Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=True, endpoint=True) target_reached=cf and node_nearby(goal_numpy__node.x, goal_numpy, np.diag([1.,1.,0.5,0.5]), np.sqrt(1.), system) if goal_check(Node(goal_numpy), real_goal_node): target_reached = target_reached and goal_check(goal_numpy__node, real_goal_node) print('target reached: %d' % (target_reached)) # might have some terminal conditions for MPNet endpoints #--- after MPNet waypoints, compute actual connection pB.reverse() # visualize pA and pB pA = np.array(pA) pB = np.array(pB) print('pa:') print(pA) print('pb:') print(pB) pA_scat = ax.scatter(pA[:,0], pA[:,1], c='lightgreen') pB_scat = ax.scatter(pB[:,0], pB[:,1], c='red') draw_update_line(ax) mpnet_path = np.concatenate([pA,pB], axis=0) mpnet_path_node = [] for i in range(len(mpnet_path)): mpnet_path_node.append(Node(mpnet_path[i])) # each time find nearest waypoint, and connect them node = start_node current_idx = 0 node_list = [node] while current_idx < len(mpnet_path)-1: # try connecting to goal x_init, u_init, t_init = init_informer(obs, node, goal_node, direction=0) next_node, edge, cf = pathSteerTo(node, Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=False, endpoint=True) if not node_nearby(edge.xs[0], node.x, np.identity(len(node.x)), 1e-2, system): next_node, edge, cf = pathSteerTo(node, Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=True, endpoint=False) if cf and goal_check(next_node, real_goal_node): node.next = next_node next_node.prev = node node.edge = edge edge.next = next_node node_list.append(next_node) break # find the min node min_d = 1e8 min_i = -1 for i in range(current_idx, len(mpnet_path)): if node_d(node.x, mpnet_path[i], np.diag([1.,1.,0.5,0.5]), system) <= min_d: min_d = node_d(node.x, mpnet_path[i], np.diag([1.,1.,0.5,0.5]), system) min_i = i if min_i == len(mpnet_path)-1: min_i = min_i - 1 if min_d > 1.: print('too far') print(min_d) node_list = node_list[:-3] break current_idx = min_i print('min_i:%d, len(mpnet_path):%d' % (min_i, len(mpnet_path))) # connect to min_i+1 x_init, u_init, t_init = init_informer(obs, node, mpnet_path_node[min_i+1], direction=0) next_node, edge, cf = pathSteerTo(node, Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=False, endpoint=False) if not node_nearby(edge.xs[0], node.x, np.identity(len(node.x)), 1e-2, system): next_node, edge, cf = pathSteerTo(node, Node(x_init[-1]), x_init, u_init, t_init, \ dynamics, enforce_bounds, IsInCollision, traj_opt, 0, system, step_sz=step_sz, num_steps=num_steps, propagating=True, endpoint=False) for i in range(len(edge.xs)): update_line(hl_for, ax, edge.xs[i]) xs_to_plot = np.array(edge.xs[::10]) for i in range(len(xs_to_plot)): xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) ax.scatter(xs_to_plot[:,0], xs_to_plot[:,1], c='g') draw_update_line(ax) animation(edge.xs, edge.us) # might add some terminal condition: if too faraway OR collision if not cf: print('in collision as cf is false') node_list = node_list[:-2] # delete the last node as it cannot steerTo next node #node_list = node_list[:-3] # delete several nodes break if goal_check(mpnet_path_node[-1], real_goal_node) and min_i+1 == len(mpnet_path)-1 and not goal_check(next_node, real_goal_node): print('endpoint not at goal') node_list = node_list[:-1] # remove last node break #next_node = Node(next_x) node.edge = edge node.next = next_node edge.next = next_node next_node.prev = node node = next_node current_idx = min_i+1 node_list.append(node) print('neural replanner over') print('len of node_list: %d' % (len(node_list))) if len(node_list) > 0: node_list[-1].edge = None # current_idx -> current_idx+1 fail, will use current_idx+2:end in mpnet_node node_list += mpnet_path_node[current_idx+2:] #node_list.append(goal_node) plt.waitforbuttonpress() pA_scat.remove() pB_scat.remove() return node_list
def plan(obs, env, x0, xG, data, informer, init_informer, system, dynamics, enforce_bounds, IsInCollision, traj_opt, jac_A, jac_B, step_sz=0.02, MAX_LENGTH=1000): # informer: given (xt, x_desired) -> x_t+1 # jac_A: given (x, u) -> linearization A # jac B: given (x, u) -> linearization B # traj_opt: given (x0, x1) -> (xs, us, dts params = {} params['obs_w'] = 6. params['obs_h'] = 6. params['integration_step'] = step_sz vis = AcrobotVisualizer(Acrobot(), params) vis.obs = obs plt.ion() fig = plt.figure() ax = fig.add_subplot(121) ax.set_autoscale_on(True) hl, = ax.plot([], [], 'b') #hl_real, = ax.plot([], [], 'r') hl_for, = ax.plot([], [], 'g') hl_back, = ax.plot([], [], 'r') hl_for_mpnet, = ax.plot([], [], 'lightgreen') hl_back_mpnet, = ax.plot([], [], 'salmon') ax_ani = fig.add_subplot(122) vis._init(ax_ani) print(obs) def update_line(h, ax, new_data): new_data = wrap_angle(new_data, system) h.set_data(np.append(h.get_xdata(), new_data[0]), np.append(h.get_ydata(), new_data[1])) #h.set_xdata(np.append(h.get_xdata(), new_data[0])) #h.set_ydata(np.append(h.get_ydata(), new_data[1])) def remove_last_k(h, ax, k): h.set_data(h.get_xdata()[:-k], h.get_ydata()[:-k]) def draw_update_line(ax): ax.relim() ax.autoscale_view() fig.canvas.draw() fig.canvas.flush_events() #plt.show() def animation(states, actions): vis._animate(states[-1], ax_ani) draw_update_line(ax_ani) dtheta = 0.1 feasible_points = [] infeasible_points = [] imin = 0 imax = int(2 * np.pi / dtheta) for i in range(imin, imax): for j in range(imin, imax): x = np.array([dtheta * i - np.pi, dtheta * j - np.pi, 0., 0.]) if IsInCollision(x): infeasible_points.append(x) else: feasible_points.append(x) feasible_points = np.array(feasible_points) infeasible_points = np.array(infeasible_points) ax.scatter(feasible_points[:, 0], feasible_points[:, 1], c='yellow') ax.scatter(infeasible_points[:, 0], infeasible_points[:, 1], c='pink') #update_line(hl, ax, x0.x) #draw_update_line(ax) for i in range(len(data)): update_line(hl, ax, data[i]) draw_update_line(ax) update_line(hl_for, ax, x0.x) draw_update_line(ax) update_line(hl_back, ax, xG.x) draw_update_line(ax) #plt.waitforbuttonpress() itr = 0 target_reached = 0 tree = 0 time_norm = 0. start = x0 goal = xG funnel_node = goal BVP_TOLERANCE = 1e-6 for_in_collision_nums = [0] # forward in collision number for_prev_scatter = [] back_in_collision_nums = [0] back_prev_scatter = [] while target_reached == 0 and itr < MAX_LENGTH: itr = itr + 1 # prevent the path from being too long print('iter: %d' % (itr)) if tree == 0: # since we ensure each step we can steer to the next waypoint # the edge connecting the two nodes will store the trajectory # information, TVLQR and the funnel size factors # the edge information is stored at the endpoint # here direciton=0 means we are computing forward steer, and 1 means # we are computing backward # the informed initialization is in the forward direction # try to connect from x0 to one node on the goal tree (nearest) node = xG min_node = node min_d = 1e6 # min_d: normalized distance xTSx / (rho^2) while node is not None: dis = h_dist(x0, node, np.identity(len(node.x)), 2., system) if dis < min_d: min_d = dis min_node = node node = node.next xw, x_init, u_init, t_init = informer(env, x0, min_node, direction=0) ax.scatter(xw.x[0], xw.x[1], c='lightgreen') draw_update_line(ax) x, e = pathSteerToBothDir(x0, xw, x_init, u_init, t_init, dynamics, enforce_bounds, IsInCollision, \ jac_A, jac_B, traj_opt, step_sz=step_sz, system=system, direction=0, propagating=True) if for_in_collision_nums[-1] >= 2 and x0.prev is not None: # backtrace, this include direct incollision nodes and indirect ones (parent) print('too many collisions... backtracing') # pop the last collision num for_in_collision_nums.pop(-1) # since the next state has collision, increase one for its parent for_in_collision_nums[-1] += 1 for_prev_scatter[-1].remove() for_prev_scatter = for_prev_scatter[:-1] # remove line as well x0 = x0.prev if x0.edge is not None: remove_last_k(hl_for, ax, len(x0.edge.xs)) draw_update_line(ax) tree = 1 itr += 1 continue if e is None: # in collision for_in_collision_nums[-1] += 1 tree = 1 itr += 1 continue # otherwise, create a new collision_num for_in_collision_nums.append(0) # if the bvp solver solution is too faraway, then ignore it # this is useful if we only use trajopt result without propagation #if np.linalg.norm(e.xs[0] - x0.x) > BVP_TOLERANCE: # # ignore it # print('forward searching bvp not successful.') # # then propagate it to obtain the result # x, e = pathSteerToBothDir(x0, xw, x_init, u_init, t_init, dynamics, enforce_bounds, IsInCollision, \ # jac_A, jac_B, traj_opt, step_sz=step_sz, system=system, direction=0, propagating=True) for i in range(len(e.xs)): update_line(hl_for, ax, e.xs[i]) xs_to_plot = np.array(e.xs[::10]) for i in range(len(xs_to_plot)): xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) for_prev_scat = ax.scatter(xs_to_plot[:, 0], xs_to_plot[:, 1], c='g') for_prev_scatter.append(for_prev_scat) draw_update_line(ax) animation(e.xs, e.us) # if connected to the min_node, then reset goal tree if h_dist(x, min_node, np.identity(len(min_node.x)), 2., system) <= 1.0: print('start tree is connected to one node in goal tree') if min_node.next is not None: xG = min_node.next #plt.waitforbuttonpress() x0.next = x x.prev = x0 e.next = x x0.edge = e x0 = x tree = 1 else: #tree=0 # skip directly #continue # the informed initialization is in the forward direction xw, x_init, u_init, t_init = informer(env, xG, x0, direction=1) # plot the informed point ax.scatter(xw.x[0], xw.x[1], c='red') draw_update_line(ax) """ if back_in_collision_nums[-1] >= 5 and xG.next is not None: # backtrace, this include direct incollision nodes and indirect ones (parent) print('backward--too many collisions... backtracing') # pop the last collision num back_in_collision_nums.pop(-1) # since the next state has collision, increase one for its parent back_in_collision_nums[-1] += 1 #back_prev_scatter[-1].remove() #back_prev_scatter = back_prev_scatter[:-1] xG = xG.next tree = 0 itr += 1 continue if IsInCollision(xw.x): # in collision back_in_collision_nums[-1] += 1 tree = 0 itr += 1 continue # otherwise, create a new collision_num back_in_collision_nums.append(0) """ #update_line(hl_back, ax, xG.x) #xs_to_plot = np.array(e.xs[::10]) #for i in range(len(xs_to_plot)): # xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) #back_prev_scat = ax.scatter(xs_to_plot[:,0], xs_to_plot[:,1], c='r') #back_prev_scatter.append(back_prev_scat) #draw_update_line(ax) #plt.waitforbuttonpress() xw.next = xG xG.prev = xw xG = xw # check if x0 can actually connect to one of the nodes in goal tree node = xG min_node = node min_d = 1e6 # min_d: normalized distance xTSx / (rho^2) while node is not None: # we need to make sure that current x0 is not near the node of interest, # to encourage exploration dis = h_dist(x0, node, np.identity(len(node.x)), 2., system) if dis < min_d and dis > 1.0: min_d = dis min_node = node node = node.next x_init, u_init, t_init = init_informer(env, x0, min_node, direction=0) x, e = pathSteerToBothDir(x0, min_node, x_init, u_init, t_init, dynamics, enforce_bounds, IsInCollision, \ jac_A, jac_B, traj_opt, step_sz=step_sz, system=system, direction=0, propagating=True) if e is None: # in collision tree = 0 itr += 1 continue if h_dist(x, min_node, np.identity(len(min_node.x)), 2., system) <= 1.0: print('start tree is connected to one node in goal tree') #if min_node.next is not None: # xG = min_node.next xG = goal #plt.waitforbuttonpress() for_in_collision_nums.append(0) for i in range(len(e.xs)): update_line(hl_for, ax, e.xs[i]) xs_to_plot = np.array(e.xs[::10]) for i in range(len(xs_to_plot)): xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) for_prev_scat = ax.scatter(xs_to_plot[:, 0], xs_to_plot[:, 1], c='g') for_prev_scatter.append(for_prev_scat) draw_update_line(ax) animation(e.xs, e.us) x0.next = x x.prev = x0 e.next = x x0.edge = e x0 = x tree = 0 # try connecting to goal x_init, u_init, t_init = init_informer(env, x0, goal, direction=0) xG_, e = pathSteerToBothDir(x0, goal, x_init, u_init, t_init, dynamics, enforce_bounds, IsInCollision, \ jac_A, jac_B, traj_opt, step_sz=step_sz, system=system, direction=0, \ propagating=True, endpoint=True) if e is not None: # check if the BVP is successful if np.linalg.norm(e.xs[0] - x0.x) > BVP_TOLERANCE: # try propagating xG_, e = pathSteerToBothDir(x0, goal, x_init, u_init, t_init, dynamics, enforce_bounds, IsInCollision, \ jac_A, jac_B, traj_opt, step_sz=step_sz, system=system, direction=0, propagating=True) #itr += 1 #continue # add xG_ to the start tree x0.next = xG_ xG_.prev = x0 x0.edge = e x0.edge.next = xG_ print('endpoint steering...') print('x0:') print(x0.x) print('goal:') print(goal.x) print('xG_:') print(xG_.x) # find the nearest point from xG_ to points on the goal tree if h_dist(xG_, goal, goal.S0, goal.rho0, system) <= 1.0: # otherwise it is a nearby node #if min_node.S0 is None: # lazyFunnel(min_node, funnel_node, dynamics, enforce_bounds, jac_A, jac_B, traj_opt, system=system, step_sz=step_sz) # #funnel_node = min_node min_node = goal reached, node_i0, node_i1 = nearby(xG_, min_node, system) if reached: target_reached = True xs_to_plot = np.array(e.xs[::10]) for i in range(len(xs_to_plot)): xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) ax.scatter(xs_to_plot[:, 0], xs_to_plot[:, 1], c='salmon') #ax.scatter(e.xs[::10,0], e.xs[::10,1], c='salmon') draw_update_line(ax) # since the nearby nodes are on the edge, need to extract the node index if min_node.edge is not None: # need to extract subset of x1.edge # change the t0 of edge starting from node to be time_knot[node_i] (use subset of edge) edge = min_node.edge edge.t0 = edge.time_knot[node_i1] edge.i0 = node_i1 # change the node to be xs[node_i], S0 to be S(time_knot[node_i]), rho0 to be rho0s[node_i] new_node = Node(wrap_angle(edge.xs[node_i1], system)) new_node.S0 = edge.S(edge.t0).reshape( (len(edge.xs[node_i1]), len(edge.xs[node_i1]))) new_node.rho0 = edge.rho0s[node_i1] new_node.edge = edge new_node.next = min_node.next min_node = new_node xG = min_node x0 = xG_ # again print out the xTSx print('xTSx:') print(node_h_dist(x0.x, xG.x, xG.S0, xG.rho0, system)) # print for the edge endpoint print('for edge endpoint:') print('xTSx/rho0^2:') print( node_h_dist(x0.prev.edge.xs[-1], xG.x, xG.S0, xG.rho0, system)) break itr += 1 if target_reached: print('target reached.') # it is near enough, so we connect in the node data structure from x0 to xG, although the endpoint of x0.edge # in state is still xG_ x0 = x0.prev # since the x0 can directly connect to xG, we only need to set the next state of the previous x to xG x0.next = xG # update endpoint (or should I?) x0.edge.next = xG xG.prev = x0 # connect the lsat node # construct the funnel later # connect from x0 to xG, the endpoint of x0 is xG_, but it is near xG print('before funnelsteerto') #lazyFunnel(start, xG, dynamics, enforce_bounds, jac_A, jac_B, traj_opt, system=system, step_sz=step_sz) #print(start.edge.rho0s) #funnelSteerTo(x0, xG, dynamics, enforce_bounds, jac_A, jac_B, traj_opt, direction=0, system=system, step_sz=step_sz) print('after funnelsteerto') #xG_.next = xG #e_.next = xG #xG_.edge = e_ else: x0.next = None x0.edge = None # construct a list of the path path_list = [] node = start while node is not None: path_list.append(node.x) node = node.next if not target_reached: # xG is the first in the goal tree while xG is not None: path_list.append(xG.x) xG = xG.next plt.cla() return target_reached, path_list
# adaptive step size on replanning attempts res, path_list = plan(obs_i, obc_i, start, goal, detail_paths, informer, init_informer, system, dynamics, \ enforce_bounds, collision_check, traj_opt, jac_A, jac_B, step_sz=step_sz, MAX_LENGTH=2000) fig = plt.figure() ax = fig.add_subplot(111) # after plan, generate the trajectory, and check if it is within the region xs = plot_trajectory(ax, start, goal, dynamics, enforce_bounds, collision_check, step_sz) params = {} params['obs_w'] = 6. params['obs_h'] = 6. params['integration_step'] = step_sz fig = plt.figure() ax = fig.add_subplot(111) animator = AcrobotVisualizer(Acrobot(), params) animation_acrobot(fig, ax, animator, xs, obs_i) plt.waitforbuttonpress() #print('after neural replan:') #print(path) #path = lvc(path, obc[i], IsInCollision, step_sz=step_sz) #print('after lvc:') #print(path) if res: fp = 1 print('feasible ok!') break #if feasibility_check(bvp_solver, path, obc_i, IsInCollision, step_sz=0.01): # fp = 1 # print('feasible, ok!') # break
def plan_mpnet(obs, env, x0, xG, data, informer, init_informer, system, dynamics, enforce_bounds, IsInCollision, traj_opt, jac_A, jac_B, step_sz=0.02, MAX_LENGTH=1000): # informer: given (xt, x_desired) -> x_t+1 # jac_A: given (x, u) -> linearization A # jac B: given (x, u) -> linearization B # traj_opt: given (x0, x1) -> (xs, us, dts params = {} params['obs_w'] = 6. params['obs_h'] = 6. params['integration_step'] = step_sz vis = AcrobotVisualizer(Acrobot(), params) vis.obs = obs plt.ion() fig = plt.figure() ax = fig.add_subplot(121) ax.set_autoscale_on(True) hl, = ax.plot([], [], 'b') #hl_real, = ax.plot([], [], 'r') hl_for, = ax.plot([], [], 'g') hl_back, = ax.plot([], [], 'r') hl_for_mpnet, = ax.plot([], [], 'lightgreen') hl_back_mpnet, = ax.plot([], [], 'salmon') ax_ani = fig.add_subplot(122) vis._init(ax_ani) print(obs) def update_line(h, ax, new_data): new_data = wrap_angle(new_data, system) h.set_data(np.append(h.get_xdata(), new_data[0]), np.append(h.get_ydata(), new_data[1])) #h.set_xdata(np.append(h.get_xdata(), new_data[0])) #h.set_ydata(np.append(h.get_ydata(), new_data[1])) def remove_last_k(h, ax, k): h.set_data(h.get_xdata()[:-k], h.get_ydata()[:-k]) def draw_update_line(ax): ax.relim() ax.autoscale_view() fig.canvas.draw() fig.canvas.flush_events() #plt.show() def animation(states, actions): vis._animate(states[-1], ax_ani) draw_update_line(ax_ani) dtheta = 0.1 feasible_points = [] infeasible_points = [] imin = 0 imax = int(2*np.pi/dtheta) for i in range(imin, imax): for j in range(imin, imax): x = np.array([dtheta*i-np.pi, dtheta*j-np.pi, 0., 0.]) if IsInCollision(x): infeasible_points.append(x) else: feasible_points.append(x) feasible_points = np.array(feasible_points) infeasible_points = np.array(infeasible_points) ax.scatter(feasible_points[:,0], feasible_points[:,1], c='yellow') ax.scatter(infeasible_points[:,0], infeasible_points[:,1], c='pink') #update_line(hl, ax, x0.x) #draw_update_line(ax) for i in range(len(data)): print(data[i]) update_line(hl, ax, data[i]) draw_update_line(ax) update_line(hl_for, ax, x0.x) draw_update_line(ax) update_line(hl_back, ax, xG.x) draw_update_line(ax) #plt.waitforbuttonpress() itr=0 target_reached=0 tree=0 time_norm = 0. start = x0 goal = xG funnel_node = goal BVP_TOLERANCE = 1e-6 for_in_collision_nums = [0] # forward in collision number for_prev_scatter = [] back_in_collision_nums = [0] back_prev_scatter = [] while target_reached==0 and itr<MAX_LENGTH: itr=itr+1 # prevent the path from being too long print('iter: %d' % (itr)) if tree==0: # since we ensure each step we can steer to the next waypoint # the edge connecting the two nodes will store the trajectory # information, TVLQR and the funnel size factors # the edge information is stored at the endpoint # here direciton=0 means we are computing forward steer, and 1 means # we are computing backward # the informed initialization is in the forward direction xw, x_init, u_init, t_init = informer(env, x0, goal, direction=0) ax.scatter(xw.x[0], xw.x[1], c='lightgreen') draw_update_line(ax) plt.waitforbuttonpress() x0 = Node(xw.x) tree=1 else: xw, x_init, u_init, t_init = informer(env, xG, start, direction=1) ax.scatter(xG.x[0], xG.x[1], c='black') ax.scatter(start.x[0], start.x[1], c='blue') ax.scatter(xw.x[0], xw.x[1], c='red') print(xw.x) draw_update_line(ax) plt.waitforbuttonpress() xG = Node(xw.x) tree=0
def plan(obs, env, x0, xG, data, informer, init_informer, system, dynamics, enforce_bounds, IsInCollisionWithObs, traj_opt, step_sz=0.02, num_steps=21, MAX_LENGTH=200, verbose=False): """ For each node xt, we record how many explorations have been made from xt. We do planning according to the following rules: 1. if n_explored >= n_max_explore: if not the root, backtrace to the previous node hat(x)_t+1 = informer(xt, G) x_t+1, tau = BVP(xt, hat(x)_t+1) n_exlored(x_t) += 1 2. if explored(x_t+1) (w.r.t. some regions as defined in SST paper) ignore x_t+1 # may also update the "score" using the already explored region 3. if too_far(x_t+1, hat(x)_t+1) ignore x_t+1 4. if inCollision(x_t+1) ignore x_t+1 # may also update the "score" of points # otherwise it is safe establish the connections between x_t and x_t+1, move to x_t+1 """ env_constr = standard_cpp_systems.RectangleObs propagate_system = env_constr(obs, 6., 'acrobot') planner = _sst_module.SSTWrapper( state_bounds=propagate_system.get_state_bounds(), control_bounds=propagate_system.get_control_bounds(), distance=propagate_system.distance_computer(), start_state=x0.x, goal_state=xG.x, goal_radius=10., random_seed=0, sst_delta_near=0.01, sst_delta_drain=0.005) obs_width = 6. new_obs_i = [] for k in range(len(obs)): obs_pt = [] obs_pt.append(obs[k][0] - obs_width / 2) obs_pt.append(obs[k][1] - obs_width / 2) obs_pt.append(obs[k][0] - obs_width / 2) obs_pt.append(obs[k][1] + obs_width / 2) obs_pt.append(obs[k][0] + obs_width / 2) obs_pt.append(obs[k][1] + obs_width / 2) obs_pt.append(obs[k][0] + obs_width / 2) obs_pt.append(obs[k][1] - obs_width / 2) new_obs_i.append(obs_pt) IsInCollision = lambda x: IsInCollisionWithObs(x, new_obs_i) # visualization if verbose: print('step_sz: %f' % (step_sz)) params = {} params['obs_w'] = 6. params['obs_h'] = 6. params['integration_step'] = step_sz vis = AcrobotVisualizer(Acrobot(), params) vis.obs = obs plt.ion() fig = plt.figure() ax = fig.add_subplot(121) ax.set_autoscale_on(True) hl, = ax.plot([], [], 'b') #hl_real, = ax.plot([], [], 'r') hl_for, = ax.plot([], [], 'g') hl_back, = ax.plot([], [], 'r') hl_for_mpnet, = ax.plot([], [], 'lightgreen') hl_back_mpnet, = ax.plot([], [], 'salmon') ax_ani = fig.add_subplot(122) vis._init(ax_ani) print(obs) def update_line(h, ax, new_data): new_data = wrap_angle(new_data, system) h.set_data(np.append(h.get_xdata(), new_data[0]), np.append(h.get_ydata(), new_data[1])) #h.set_xdata(np.append(h.get_xdata(), new_data[0])) #h.set_ydata(np.append(h.get_ydata(), new_data[1])) def remove_last_k(h, ax, k): h.set_data(h.get_xdata()[:-k], h.get_ydata()[:-k]) def draw_update_line(ax): ax.relim() ax.autoscale_view() fig.canvas.draw() fig.canvas.flush_events() #plt.show() def animation(states, actions): vis._animate(states[-1], ax_ani) draw_update_line(ax_ani) dtheta = 0.1 feasible_points = [] infeasible_points = [] goal_region = [] imin = 0 imax = int(2 * np.pi / dtheta) for i in range(imin, imax): for j in range(imin, imax): x = np.array([dtheta * i - np.pi, dtheta * j - np.pi, 0., 0.]) if IsInCollision(x): infeasible_points.append(x) else: feasible_points.append(x) if goal_check(Node(x), xG, system): goal_region.append(x) feasible_points = np.array(feasible_points) infeasible_points = np.array(infeasible_points) goal_region = np.array(goal_region) ax.scatter(feasible_points[:, 0], feasible_points[:, 1], c='yellow') ax.scatter(infeasible_points[:, 0], infeasible_points[:, 1], c='pink') ax.scatter(goal_region[:, 0], goal_region[:, 1], c='green') for i in range(len(data)): update_line(hl, ax, data[i]) draw_update_line(ax) update_line(hl_for, ax, x0.x) draw_update_line(ax) update_line(hl_back, ax, xG.x) draw_update_line(ax) # visualization end env = torch.FloatTensor(env) env = to_var(env) #explored_nodes = [x0] xt = x0 xt.cost = 0. fes = False for itr in range(MAX_LENGTH): # pop nodes from frontier_nodes if xt is None: xt = x0 #print('start state:') #print(xt.x) # try connecting to goal every now and then x_init, u_init, t_init = init_informer(env, xt, xG, direction=0) x_G_, edge, valid = pathSteerTo(xt, xG, x_init, u_init, t_init, dynamics, enforce_bounds, IsInCollision, \ traj_opt, step_sz=step_sz, num_steps=num_steps, system=system, direction=0, propagating=True) # visualization if verbose: xs_to_plot = np.array(edge.xs[::10]) for i in range(len(xs_to_plot)): xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) ax.scatter(xs_to_plot[:, 0], xs_to_plot[:, 1], c='orange') draw_update_line(ax) # visualization end if edge is not None: # check goal for each node for i in range(len(edge.xs)): if goal_check(Node(wrap_angle(edge.xs[i], system)), xG, system): print('bingo') return 1 xt.edge = None xt.next = None xs, us, dts = planner.step_bvp(propagate_system, system, xt.x, x_init[-1], 400, num_steps, step_sz, x_init, u_init, t_init) if len(us) != 0: #xs_to_plot = np.array(edge.xs[::10]) #for i in range(len(xs_to_plot)): # xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) x_t_1 = xs[-1] # visualization if verbose: ax.scatter(x_t_1[0], x_t_1[1], c='orange') #ax.scatter(xs_to_plot[:,0], xs_to_plot[:,1], c='orange') draw_update_line(ax) # visualization end edge_dt = np.sum(dts) start = xt goal = Node(wrap_angle(xs[-1], system)) # after trajopt, make actions of dimension 2 us = us.reshape(len(us), -1) # notice that controller time starts from 0, hence locally need to shift the time by minusing t0_edges # start from 0 time_knot = np.cumsum(dts) time_knot = np.insert(time_knot, 0, 0.) # can also change the resolution by the following function (for instance, every 10) edge = Edge(xs, us, dts, time_knot, edge_dt) edge.next = goal start.edge = edge start.next = goal goal.prev = start #goal.n_explored = 0 #goal.cost = xt.cost + xt.edge.dt """ # push to frontier tie_breaker += 1 entry = (goal.cost+h_cost(goal,xG,system), tie_breaker, goal) heapq.heappush(frontier_nodes, entry) if goal_check(Node(x_t_1), xG, system): print('bingo!') fes = True break """ xw, x_init, u_init, t_init = informer(env, xt, xG, direction=0) # visualization if verbose: xw_scat = ax.scatter(xw.x[0], xw.x[1], c='lightgreen') draw_update_line(ax) # visualization end xs, us, dts = planner.step_bvp(propagate_system, system, xt.x, xw.x, 400, num_steps, step_sz, x_init, u_init, t_init) #print('xs:') #print(xs) # stop at the last node that is not in collision if len(us) == 0: print('step_bvp failed') # the same node, it hasn't changed xt = None continue # establish connections to x_t+1 edge_dt = np.sum(dts) goal = Node(wrap_angle(xs[-1], system)) # after trajopt, make actions of dimension 2 us = us.reshape(len(us), -1) # notice that controller time starts from 0, hence locally need to shift the time by minusing t0_edges # start from 0 time_knot = np.cumsum(dts) time_knot = np.insert(time_knot, 0, 0.) xt = goal #for i in range(len(edge.xs)): # update_line(hl_for, ax, edge.xs[i]) # visualization if verbose: xs_to_plot = np.array(xs[::5]) for i in range(len(xs_to_plot)): xs_to_plot[i] = wrap_angle(xs_to_plot[i], system) ax.scatter(xs_to_plot[:, 0], xs_to_plot[:, 1], c='g') draw_update_line(ax) animation(xs, us) # visualization end # check if the new node is near goal if goal_check(goal, xG, system): print("success") fes = True break if fes: break # check if the new node is near goal if goal_check(xt, xG, system): print("success") fes = True break return fes
def eval_tasks_mpnet(mpNet0, mpNet1, env_type, test_data, save_dir, data_type, normalize_func=lambda x: x, unnormalize_func=lambda x: x, dynamics=None, jac_A=None, jac_B=None, enforce_bounds=None, IsInCollision=None): # data_type: seen or unseen obc, obs, paths, sgs, path_lengths, controls, costs = test_data if obs is not None: obc = obc.astype(np.float32) obc = torch.from_numpy(obc) if torch.cuda.is_available(): obc = obc.cuda() if env_type == 'pendulum': system = standard_cpp_systems.PSOPTPendulum() bvp_solver = _sst_module.PSOPTBVPWrapper(system, 2, 1, 0) step_sz = 0.002 num_steps = 20 elif env_type == 'cartpole_obs': #system = standard_cpp_systems.RectangleObs(obs[i], 4.0, 'cartpole') system = _sst_module.CartPole() bvp_solver = _sst_module.PSOPTBVPWrapper(system, 4, 1, 0) step_sz = 0.002 num_steps = 20 goal_S0 = np.identity(4) goal_rho0 = 1.0 elif env_type in [ 'acrobot_obs', 'acrobot_obs_2', 'acrobot_obs_3', 'acrobot_obs_4', 'acrobot_obs_8' ]: #system = standard_cpp_systems.RectangleObs(obs[i], 6.0, 'acrobot') obs_width = 6.0 system = _sst_module.PSOPTAcrobot() bvp_solver = _sst_module.PSOPTBVPWrapper(system, 4, 1, 0) step_sz = 0.02 num_steps = 20 goal_S0 = np.diag([1., 1., 0, 0]) #goal_S0 = np.identity(4) goal_rho0 = 1.0 circular = system.is_circular_topology() def informer(env, x0, xG, direction): x0_x = torch.from_numpy(x0.x).type(torch.FloatTensor) xG_x = torch.from_numpy(xG.x).type(torch.FloatTensor) x0_x = normalize_func(x0_x) xG_x = normalize_func(xG_x) if torch.cuda.is_available(): x0_x = x0_x.cuda() xG_x = xG_x.cuda() if direction == 0: x = torch.cat([x0_x, xG_x], dim=0) mpNet = mpNet0 if torch.cuda.is_available(): x = x.cuda() next_state = mpNet(x.unsqueeze(0), env.unsqueeze(0)).cpu().data print('next state:') print(next_state) next_state = unnormalize_func(next_state).numpy()[0] print('after unnormalize:') print(next_state) delta_x = next_state - x0.x # can be either clockwise or counterclockwise, take shorter one for i in range(len(delta_x)): if circular[i]: delta_x[i] = delta_x[i] - np.floor( delta_x[i] / (2 * np.pi)) * (2 * np.pi) if delta_x[i] > np.pi: delta_x[i] = delta_x[i] - 2 * np.pi # randomly pick either direction rand_d = np.random.randint(2) if rand_d < 1: if delta_x[i] > 0.: delta_x[i] = delta_x[i] - 2 * np.pi if delta_x[i] <= 0.: delta_x[i] = delta_x[i] + 2 * np.pi res = Node(next_state) x_init = np.linspace(x0.x, x0.x + delta_x, num_steps) ## TODO: : change this to general case u_init_i = np.random.uniform(low=[-4.], high=[4]) #u_init_i = control[max_d_i] cost_i = num_steps * step_sz u_init = np.repeat(u_init_i, num_steps, axis=0).reshape(-1, len(u_init_i)) u_init = u_init + np.random.normal(scale=1.) t_init = np.linspace(0, cost_i, num_steps) else: x = torch.cat([x0_x, xG_x], dim=0) mpNet = mpNet1 next_state = mpNet(x.unsqueeze(0), env.unsqueeze(0)).cpu().data print('next state:') print(next_state) next_state = unnormalize_func(next_state).numpy()[0] print('after unnormalize:') print(next_state) delta_x = x0.x - next_state # can be either clockwise or counterclockwise, take shorter one for i in range(len(delta_x)): if circular[i]: delta_x[i] = delta_x[i] - np.floor( delta_x[i] / (2 * np.pi)) * (2 * np.pi) if delta_x[i] > np.pi: delta_x[i] = delta_x[i] - 2 * np.pi # randomly pick either direction rand_d = np.random.randint(2) if rand_d < 1: if delta_x[i] > 0.: delta_x[i] = delta_x[i] - 2 * np.pi elif delta_x[i] <= 0.: delta_x[i] = delta_x[i] + 2 * np.pi #next_state = state[max_d_i] + delta_x res = Node(next_state) # initial: from max_d_i to max_d_i+1 x_init = np.linspace(next_state, next_state + delta_x, num_steps) # + rand_x_init # action: copy over to number of steps u_init_i = np.random.uniform(low=[-4.], high=[4]) cost_i = num_steps * step_sz u_init = np.repeat(u_init_i, num_steps, axis=0).reshape(-1, len(u_init_i)) u_init = u_init + np.random.normal(scale=1.) t_init = np.linspace(0, cost_i, num_steps) return res, x_init, u_init, t_init def init_informer(env, x0, xG, direction): if direction == 0: next_state = xG.x delta_x = next_state - x0.x # can be either clockwise or counterclockwise, take shorter one for i in range(len(delta_x)): if circular[i]: delta_x[i] = delta_x[i] - np.floor( delta_x[i] / (2 * np.pi)) * (2 * np.pi) if delta_x[i] > np.pi: delta_x[i] = delta_x[i] - 2 * np.pi # randomly pick either direction rand_d = np.random.randint(2) if rand_d < 1: if delta_x[i] > 0.: delta_x[i] = delta_x[i] - 2 * np.pi if delta_x[i] <= 0.: delta_x[i] = delta_x[i] + 2 * np.pi res = Node(next_state) x_init = np.linspace(x0.x, x0.x + delta_x, num_steps) ## TODO: : change this to general case u_init_i = np.random.uniform(low=[-4.], high=[4]) #u_init_i = control[max_d_i] cost_i = num_steps * step_sz u_init = np.repeat(u_init_i, num_steps, axis=0).reshape(-1, len(u_init_i)) u_init = u_init + np.random.normal(scale=1.) t_init = np.linspace(0, cost_i, num_steps) else: next_state = xG.x delta_x = x0.x - next_state # can be either clockwise or counterclockwise, take shorter one for i in range(len(delta_x)): if circular[i]: delta_x[i] = delta_x[i] - np.floor( delta_x[i] / (2 * np.pi)) * (2 * np.pi) if delta_x[i] > np.pi: delta_x[i] = delta_x[i] - 2 * np.pi # randomly pick either direction rand_d = np.random.randint(2) if rand_d < 1: if delta_x[i] > 0.: delta_x[i] = delta_x[i] - 2 * np.pi elif delta_x[i] <= 0.: delta_x[i] = delta_x[i] + 2 * np.pi #next_state = state[max_d_i] + delta_x res = Node(next_state) # initial: from max_d_i to max_d_i+1 x_init = np.linspace(next_state, next_state + delta_x, num_steps) # + rand_x_init # action: copy over to number of steps u_init_i = np.random.uniform(low=[-4.], high=[4]) cost_i = num_steps * step_sz u_init = np.repeat(u_init_i, num_steps, axis=0).reshape(-1, len(u_init_i)) u_init = u_init + np.random.normal(scale=1.) t_init = np.linspace(0, cost_i, num_steps) return x_init, u_init, t_init fes_env = [] # list of list valid_env = [] time_env = [] time_total = [] for i in range(len(paths)): time_path = [] fes_path = [] # 1 for feasible, 0 for not feasible valid_path = [] # if the feasibility is valid or not # save paths to different files, indicated by i #print(obs, flush=True) # feasible paths for each env for j in range(len(paths[0])): state_i = [] state = paths[i][j] # obtain the sequence p_start = paths[i][j][0] detail_paths = [p_start] detail_controls = [] detail_costs = [] state = [p_start] control = [] cost = [] for k in range(len(controls[i][j])): #state_i.append(len(detail_paths)-1) max_steps = int(costs[i][j][k] / step_sz) accum_cost = 0. print('p_start:') print(p_start) print('data:') print(paths[i][j][k]) # modify it because of small difference between data and actual propagation p_start = paths[i][j][k] state[-1] = paths[i][j][k] for step in range(1, max_steps + 1): p_start = dynamics(p_start, controls[i][j][k], step_sz) p_start = enforce_bounds(p_start) detail_paths.append(p_start) detail_controls.append(controls[i][j]) detail_costs.append(step_sz) accum_cost += step_sz if (step % 20 == 0) or (step == max_steps): state.append(p_start) print('control') print(controls[i][j]) control.append(controls[i][j][k]) cost.append(accum_cost) accum_cost = 0. print('p_start:') print(p_start) print('data:') print(paths[i][j][-1]) state[-1] = paths[i][j][-1] ############################# time0 = time.time() time_norm = 0. fp = 0 # indicator for feasibility print("step: i=" + str(i) + " j=" + str(j)) p1_ind = 0 p2_ind = 0 p_ind = 0 if path_lengths[i][j] == 0: # invalid, feasible = 0, and path count = 0 fp = 0 valid_path.append(0) if path_lengths[i][j] > 0: fp = 0 valid_path.append(1) #paths[i][j][0][1] = 0. #paths[i][j][path_lengths[i][j]-1][1] = 0. path = [paths[i][j][0], paths[i][j][path_lengths[i][j] - 1]] # plot the entire path #plt.plot(paths[i][j][:,0], paths[i][j][:,1]) start = Node(path[0]) #goal = Node(path[-1]) goal = Node(sgs[i][j][1]) goal.S0 = goal_S0 goal.rho0 = goal_rho0 # change this later control = [] time_step = [] step_sz = step_sz MAX_NEURAL_REPLAN = 11 if obs is None: obs_i = None obc_i = None else: obs_i = obs[i] obc_i = obc[i] # convert obs_i center to points new_obs_i = [] for k in range(len(obs_i)): obs_pt = [] obs_pt.append(obs_i[k][0] - obs_width / 2) obs_pt.append(obs_i[k][1] - obs_width / 2) obs_pt.append(obs_i[k][0] - obs_width / 2) obs_pt.append(obs_i[k][1] + obs_width / 2) obs_pt.append(obs_i[k][0] + obs_width / 2) obs_pt.append(obs_i[k][1] + obs_width / 2) obs_pt.append(obs_i[k][0] + obs_width / 2) obs_pt.append(obs_i[k][1] - obs_width / 2) new_obs_i.append(obs_pt) #obs_i = new_obs_i collision_check = lambda x: IsInCollision(x, new_obs_i) for t in range(MAX_NEURAL_REPLAN): # adaptive step size on replanning attempts res, path_list = plan_mpnet(obs_i, obc_i, start, goal, detail_paths, informer, init_informer, system, dynamics, \ enforce_bounds, collision_check, None, jac_A, jac_B, step_sz=step_sz, MAX_LENGTH=1000) fig = plt.figure() ax = fig.add_subplot(111) # after plan, generate the trajectory, and check if it is within the region xs = plot_trajectory(ax, start, goal, dynamics, enforce_bounds, collision_check, step_sz) params = {} params['obs_w'] = 6. params['obs_h'] = 6. params['integration_step'] = step_sz fig = plt.figure() ax = fig.add_subplot(111) animator = AcrobotVisualizer(Acrobot(), params) animation_acrobot(fig, ax, animator, xs, obs_i) plt.waitforbuttonpress() #print('after neural replan:') #print(path) #path = lvc(path, obc[i], IsInCollision, step_sz=step_sz) #print('after lvc:') #print(path) if res: fp = 1 print('feasible ok!') break #if feasibility_check(bvp_solver, path, obc_i, IsInCollision, step_sz=0.01): # fp = 1 # print('feasible, ok!') # break if fp: # only for successful paths time1 = time.time() - time0 time1 -= time_norm time_path.append(time1) print('test time: %f' % (time1)) # write the path #print('planned path:') #print(path) #path = np.array(path) #np.savetxt('results/path_%d.txt' % (j), path) #np.savetxt('results/control_%d.txt' % (j), np.array(control)) #np.savetxt('results/timestep_%d.txt' % (j), np.array(time_step)) fes_path.append(fp) time_env.append(time_path) time_total += time_path print('average test time up to now: %f' % (np.mean(time_total))) fes_env.append(fes_path) valid_env.append(valid_path) print('accuracy up to now: %f' % (float(np.sum(fes_env)) / np.sum(valid_env))) time_path = save_dir + 'mpnet_%s_time.pkl' % (data_type) pickle.dump(time_env, open(time_path, "wb")) #print(fp/tp) return np.array(fes_env), np.array(valid_env)