for t in xrange(T-1): X[:,t+1] = As[:,:,t]*(X[:,t]-X_bar[:,t]) + Bs[:,:,t]*(U_bar[:,t]-U_bar[:,t]) +\ Cs[:,t] s.draw() car.draw_trajectory(mat2tuple(X.T)) plt.show() ''' exit() # Apply SCP rho_x = 0.1 rho_u = 0.1 N_iter = 5 opt_states, opt_ctrls, opt_vals = scp_solver_states(links, X_bar, U_bar, rho_x, rho_u,\ N_iter, method='shooting') # Plot states obtained by applying returned optimal controls X = np.mat(np.zeros((links.NX, T))) opt_ctrls = np.mat(opt_ctrls) X[:, 0] = X_bar[:, 0] print opt_ctrls.shape print opt_ctrls for t in xrange(0, T - 1): X[:, t + 1] = links.dynamics(X[:, t], opt_ctrls[:, t]) links.draw_trajectory(mat2tuple(X.T), color='blue') plt.show()
U = numpy.mat(numpy.random.random_sample((arm.NU, T - 1)) / 5) U[:, 10:] = -2 * U[:, 10:] for t in range(T - 1): #U[:,t] = du.T X[:, t + 1] = arm.dynamics(X[:, t], U[:, t]) arm.draw_trajectory(X) rho_x = 0.1 rho_u = 1.0 N_iter = 4 print X X_copy = X.copy() U_copy = U.copy() opt_states, opt_ctrls, opt_vals = scp_solver_states(arm, X_copy, U_copy, rho_x, rho_u,\ N_iter, method='shooting') arm.draw_trajectory(opt_states, color=numpy.array((0.0, 1.0, 0.0))) drawRobot = opt_states getch = _Getch._Getch() t = 0 while True: c = getch() print c if c == 'x': break elif ord(c) == 3 or ord(c) == 4: exit(0) elif c == 'n': print 'drawRobot = X'
for t in xrange(T-1): X[:,t+1] = As[:,:,t]*(X[:,t]-X_bar[:,t]) + Bs[:,:,t]*(U_bar[:,t]-U_bar[:,t]) +\ Cs[:,t] s.draw() car.draw_trajectory(mat2tuple(X.T)) plt.show() ''' exit() # Apply SCP rho_x = 0.1 rho_u = 0.1 N_iter = 5 opt_states, opt_ctrls, opt_vals = scp_solver_states(links, X_bar, U_bar, rho_x, rho_u,\ N_iter, method='shooting') # Plot states obtained by applying returned optimal controls X = np.mat(np.zeros((links.NX, T))) opt_ctrls = np.mat(opt_ctrls) X[:,0] = X_bar[:,0] print opt_ctrls.shape print opt_ctrls for t in xrange(0,T-1): X[:,t+1] = links.dynamics(X[:,t], opt_ctrls[:,t]) links.draw_trajectory(mat2tuple(X.T), color='blue') plt.show()
U = numpy.mat(numpy.random.random_sample((arm.NU, T-1))/5) U[:,10:] = -2 * U[:,10:] for t in range(T-1): #U[:,t] = du.T X[:,t+1] = arm.dynamics(X[:,t], U[:,t]) arm.draw_trajectory(X) rho_x = 0.1 rho_u = 1.0 N_iter = 4 print X X_copy = X.copy() U_copy = U.copy() opt_states, opt_ctrls, opt_vals = scp_solver_states(arm, X_copy, U_copy, rho_x, rho_u,\ N_iter, method='shooting') arm.draw_trajectory(opt_states, color=numpy.array((0.0,1.0,0.0))) drawRobot = opt_states getch = _Getch._Getch() t = 0 while True: c = getch() print c if c == 'x': break; elif ord(c) == 3 or ord(c) == 4: exit(0) elif c == 'n': print 'drawRobot = X'