Sigmas = np.zeros((Q.shape[0], Q.shape[1], T))
Sigmas[:, :, 0] = np.mat(np.diag([0.002] * num_states))  # arg
# 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
Sigmas = np.zeros((Q.shape[0], Q.shape[1],T))
Sigmas[:,:,0] = np.mat(np.diag([0.002]*num_states)) #arg
#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;
Exemple #3
0
Sigmas = np.zeros((Q.shape[0], Q.shape[1], T))
Sigmas[:, :, 0] = np.mat(np.diag([0.002] * num_states))  #arg
#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':
x0 = numpy.array([0.0] * arm.NX)
du = numpy.array([0.0] * arm.NU)
du = numpy.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])
du = numpy.mat(du)
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5

T = 20
X = numpy.mat(numpy.zeros((arm.NX, T)))
U = numpy.mat(numpy.zeros((arm.NU, T - 1)))
U = numpy.mat(numpy.random.random_sample((arm.NU, T - 1)) / 10)
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 = 0.1
N_iter = 5
print X
X_copy = X.copy()
U_copy = U.copy()

cost_t_fns = []
for t in range(T - 1):
    arm_pos = arm.traj_pos(X[:, t])
    cost_t_fns.append(lambda x, u, pos=arm_pos.copy(): cost_t(x, u, pos))
Exemple #5
0
Sigmas = np.zeros((Q.shape[0], Q.shape[1], T))
Sigmas[:, :, 0] = np.mat(np.diag([0.002] * num_states))  #arg
#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
x0 = numpy.array([0.0] * arm.NX)
du = numpy.array([0.0] * arm.NU) 
du = numpy.array([0.0, 0.1, -0.02, -0.05, 0.04, 0.02, 0.1])
du = numpy.mat(du)
#U_bar = np.mat(np.random.random_sample((car.NU, T-1)))/5


T = 20
X = numpy.mat(numpy.zeros((arm.NX, T)))
U = numpy.mat(numpy.zeros((arm.NU, T-1)))
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