links = Links(x0) s.add_robot(links) T = 50 X_bar = np.mat(np.zeros((links.NX, T))) U_bar = np.mat(np.random.random_sample((links.NU, T - 1))) / 2 print U_bar for t in xrange(1, T): X_bar[:, t] = links.dynamics(X_bar[:, t - 1], U_bar[:, t - 1]) # Plot nominal trajectory ax = plt.gca() s.draw(ax=ax) links.draw_trajectory(mat2tuple(X_bar.T)) #plt.show() #stop ''' X = np.mat(np.zeros((car.NX, T))) As, Bs, Cs = car.linearize_dynamics_trajectory(X_bar, U_bar) 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()
#U_bar = 10*np.mat(np.random.random_sample((links.NU, T-1))) - 5 for t in xrange(1,T): U_bar[0, t-1] = 1*float(t)/T U_bar[1, t-1] = 1.2 X_bar[:,t] = links.dynamics(X_bar[:,t-1], U_bar[:, t-1]) mus[:,t], Sigmas[:,:,t] = ekf_update(links.dynamics, lambda x: links.observe(s, x=x), Q, R, mus[:,t-1], Sigmas[:,:,t-1], U_bar[:,t-1], None) # Plot nominal trajectory #ax = plt.subplot(121) ax = plt.gca() s.draw(ax=ax) links.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2,0:2,:], color='red') #plt.show() #stop Bel_bar = np.mat(np.zeros((links.NB, T))) for t in xrange(T): Bel_bar[:,t] = np.vstack((X_bar[:,t], cov2vec(Sigmas[:,:,t]))) goal_bel = np.copy(Bel_bar[:,-1]) #goal_bel[0:links.NX] = xN; goal_bel[links.NX:] = 0 # Apply SCP rho_bel = 0.1 rho_u = 0.1
links = Links(x0) s.add_robot(links) T = 50 X_bar = np.mat(np.zeros((links.NX, T))) U_bar = np.mat(np.random.random_sample((links.NU, T-1)))/2 print U_bar for t in xrange(1,T): X_bar[:,t] = links.dynamics(X_bar[:,t-1], U_bar[:, t-1]) # Plot nominal trajectory ax = plt.gca() s.draw(ax=ax) links.draw_trajectory(mat2tuple(X_bar.T)) #plt.show() #stop ''' X = np.mat(np.zeros((car.NX, T))) As, Bs, Cs = car.linearize_dynamics_trajectory(X_bar, U_bar) 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()
U_bar[0, t - 1] = 1 * float(t) / T U_bar[1, t - 1] = 1.2 X_bar[:, t] = links.dynamics(X_bar[:, t - 1], U_bar[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update(links.dynamics, lambda x: links.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U_bar[:, t - 1], None) # Plot nominal trajectory #ax = plt.subplot(121) ax = plt.gca() s.draw(ax=ax) links.draw_trajectory(mat2tuple(X_bar.T), mus=X_bar, Sigmas=Sigmas[0:2, 0:2, :], color='red') #plt.show() #stop Bel_bar = np.mat(np.zeros((links.NB, T))) for t in xrange(T): Bel_bar[:, t] = np.vstack((X_bar[:, t], cov2vec(Sigmas[:, :, t]))) goal_bel = np.copy(Bel_bar[:, -1]) #goal_bel[0:links.NX] = xN; goal_bel[links.NX:] = 0 # Apply SCP rho_bel = 0.1 rho_u = 0.1