# Sigmas[2,2,0] = 0.0000001 X = np.mat(np.zeros((arm.NX, T))) X[:, 0] = np.mat(x0).T U = np.mat(np.zeros((arm.NU, T - 1))) for t in range(T - 1): U[:, t] = du # U[:,T/2:] = -2 * U[:,T/2:] # U = np.mat(np.random.random_sample((arm.NU, T-1))/5) # U[:,10:] = -2 * U[:,10:] for t in xrange(1, T): X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update( arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None ) # NOTE No obs arm.draw_trajectory(X, mus, Sigmas) Bel_bar = np.mat(np.zeros((arm.NB, T))) for t in xrange(T): Bel_bar[:, t] = np.vstack((X[:, t], cov2vec(Sigmas[:, :, t]))) rho_bel = 0.2 rho_u = 0.3 N_iter = 1 goal_bel = np.copy(Bel_bar[:, -1]) goal_bel[arm.NX :] = 0 opt_bels, opt_ctrls, opt_vals = scp_solver_beliefs(
X = np.mat(np.zeros((arm.NX, T))) X[:, 0] = np.mat(x0).T U = np.mat(np.zeros((arm.NU, T - 1))) for t in range(T - 1): U[:, t] = du #U[:,T/2:] = -2 * U[:,T/2:] #U = np.mat(np.random.random_sample((arm.NU, T-1))/5) #U[:,10:] = -2 * U[:,10:] for t in xrange(1, T): X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update(arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None) #NOTE No obs arm.draw_trajectory(X, mus, Sigmas) drawRobot = X 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)
#Sigmas[2,2,0] = 0.0000001 X = np.mat(np.zeros((arm.NX, T))) X[:,0] = np.mat(x0).T U = np.mat(np.zeros((arm.NU, T-1))) for t in range(T-1): U[:,t] = du #U[:,T/2:] = -2 * U[:,T/2:] #U = np.mat(np.random.random_sample((arm.NU, T-1))/5) #U[:,10:] = -2 * U[:,10:] for t in xrange(1,T): X[:,t] = arm.dynamics(X[:,t-1], U[:, t-1]) mus[:,t], Sigmas[:,:,t] = ekf_update(arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:,t-1], Sigmas[:,:,t-1], U[:,t-1], None) #NOTE No obs arm.draw_trajectory(X, mus, Sigmas) drawRobot = X 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)
X = np.mat(np.zeros((arm.NX, T))) X[:, 0] = np.mat(x0).T U = np.mat(np.zeros((arm.NU, T - 1))) for t in range(T - 1): U[:, t] = du #U[:,T/2:] = -2 * U[:,T/2:] #U = np.mat(np.random.random_sample((arm.NU, T-1))/5) #U[:,10:] = -2 * U[:,10:] for t in xrange(1, T): X[:, t] = arm.dynamics(X[:, t - 1], U[:, t - 1]) mus[:, t], Sigmas[:, :, t] = ekf_update(arm.dynamics, lambda x: arm.observe(s, x=x), Q, R, mus[:, t - 1], Sigmas[:, :, t - 1], U[:, t - 1], None) #NOTE No obs arm.draw_trajectory(X, mus, Sigmas) Bel_bar = np.mat(np.zeros((arm.NB, T))) for t in xrange(T): Bel_bar[:, t] = np.vstack((X[:, t], cov2vec(Sigmas[:, :, t]))) rho_bel = 0.2 rho_u = 0.3 N_iter = 1 goal_bel = np.copy(Bel_bar[:, -1]) goal_bel[arm.NX:] = 0