# Set up environment
s = SimEnv2D(bounds=[-1, 1, -1, 1])
x0 = np.array([0.1, 0.2])
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()
# Set up environment
s = SimEnv2D(bounds=[-1, 1, -1, 1])
x0 = np.array([0.1, 0.2])
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()
# Attach sensor to elbow of arm facing direction of second link
def pos_fn(x):
return x[0] + cos(x[2]+pi/2)*l1,\
x[1] + sin(x[2]+pi/2)*l1,\
x[2] + x[3]
two_link.attach_sensor(cam, pos_fn)
s.add_robot(two_link)
# Draw scene
s.draw()
plt.gca().set_xlim(-1, 1);
plt.gca().set_ylim(-1, 1);
# Generate a nominal trajectory
T = 100
X_bar = mat(zeros((two_link.NX, T)))
U_bar = mat(ones((two_link.NU, T-1))/10)
for t in xrange(T):
X_bar[:,t] = two_link.dynamics(X_bar[:,t-1], U_bar[:, t-1])
#print X_bar[:,T-1]
# Trust region
rho_x = 1000.0
rho_u = 10.0
opt_states, opt_controls = scp_solver(two_link, X_bar, U_bar, rho_x, rho_u, 1)
mus = np.mat(np.zeros((num_states,T)))
mus[:,0] = x0;
Sigmas = np.zeros((Q.shape[0], Q.shape[1],T))
Sigmas[:,:,0] = np.mat(np.diag([0.005]*num_states)) #arg
#Sigmas[2,2,0] = 0.0000001
#Sigmas[3,3,0] = 0.0000001
# Generate a nomimal trajectory
X_bar = np.mat(np.zeros((links.NX, T)))
X_bar[:,0] = x0;
U_bar = np.mat(np.zeros((links.NU, T-1)))
#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)))
mus = np.mat(np.zeros((num_states, T)))
mus[:, 0] = x0
Sigmas = np.zeros((Q.shape[0], Q.shape[1], T))
Sigmas[:, :, 0] = np.mat(np.diag([0.005] * num_states)) #arg
#Sigmas[2,2,0] = 0.0000001
#Sigmas[3,3,0] = 0.0000001
# Generate a nomimal trajectory
X_bar = np.mat(np.zeros((links.NX, T)))
X_bar[:, 0] = x0
U_bar = np.mat(np.zeros((links.NU, T - 1)))
#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')
