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'