# 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
