def eef_velocity_2dof(self):
plt.figure(figsize=(10, 6))
load.init_load("2dof_time.csv", force=True)
_, labels = load.training_set()
n = 8000
dof = 2
state_f = labels[:, 2 * dof:]
knn = NearestNeighbors(n_neighbors=n).fit(state_f)
d, v_idx = knn.kneighbors([[0.25 * np.pi, 0 * np.pi, 0.0, 0.0]])
# idx = v_idx
idx = v_idx.reshape(-1)[np.flatnonzero(d.reshape(-1) < 0.30)]
nn = state_f[idx.reshape(-1), :]
plt.subplot(1, 2, 1)
x1 = np.cos(nn[:, 0]) + np.cos(nn[:, 1])
y1 = 1.0 * (np.sin(nn[:, 0]) + np.sin(nn[:, 1]))
omega = np.linalg.norm(nn[:, dof:], axis=1)
plt.scatter(x1, y1, s=1, alpha=0.5, c=omega)
plt.colorbar().set_label('Norm of omega')
plt.axis('equal')
plt.title('{} nearest neighbors to goal state'.format(idx.shape[0]))
plt.xlabel("x")
plt.ylabel("y")
plt.subplot(1, 2, 2)
plt.hist(d.reshape(-1), bins='auto')
plt.title('Euclidean distance to goal')
plt.xlabel('d')
plt.show()
def eef_velocity_1dof(self):
plt.figure(figsize=(10, 6))
load.init_load("pendulum_time.csv", force=True)
_, labels = load.training_set()
dof = 1
state_f = labels[:, 2 * dof:]
n = 1000
knn = NearestNeighbors(n_neighbors=1000).fit(state_f[:, :2 * dof])
d, v_idx = knn.kneighbors([[0, 0]])
idx = v_idx.reshape(-1)[np.flatnonzero(d.reshape(-1) < 0.15)]
# idx= v_idx
nn = state_f[idx.reshape(-1), :]
plt.subplot(1, 2, 1)
x1 = np.sin(nn[:, 0]) # + np.cos(nn[:, 1])
y1 = 1.0 * (np.cos(nn[:, 0])) # + np.sin(nn[:, 1]))
omega = np.linalg.norm(nn[:, dof:], axis=1)
plt.scatter(x1, y1, s=1, alpha=0.5, c=omega)
plt.colorbar().set_label('Norm of omega')
plt.axis('equal')
plt.title('Neighbors within 0.15 to goal state')
plt.xlabel("x")
plt.ylabel("y")
plt.subplot(1, 2, 2)
plt.hist(d.reshape(-1), bins='auto')
plt.title('Euclidean distance to goal')
plt.xlabel('d')
return None
def phase_scatter(self):
load.init_load()
data, labels = load.training_set()
k = 10000
idx = np.random.randint(0, labels.shape[0], k)
dof = self.cfg.simulation.dof
state_i = labels[idx, :2 * dof]
state_f = labels[idx, 2 * dof:]
omega_norm = np.linalg.norm(state_f[:, 2:], axis=1)
# Plot first link
plt.subplot(1, 2, 1)
plt.scatter(state_f[:, 0], state_f[:, 2], s=1, alpha=0.5, c=omega_norm)
plt.axis("equal")
plt.xlabel("theta1_f")
plt.ylabel("omega1_f")
# Plot second link
plt.subplot(1, 2, 2)
plt.scatter(state_f[:, 1], state_f[:, 3], s=1, alpha=0.5, c=omega_norm)
plt.axis("equal")
plt.colorbar().set_label("Norm of final velocity")
plt.xlabel("theta2_f")
plt.ylabel("omega2_f")
plt.show()
return None
def load_data(self):
print("Loading data...")
try:
load.init_load(force=True)
except ValueError as e:
print(e)
return False
return True
def eef_pos_2dof(self):
"""
Display k=5000 configurations of the end effector
"""
plt.figure(figsize=(10, 6))
load.init_load("2dof_time.csv", force=True)
_, labels = load.training_set()
# idx = np.random.randint(0, labels.shape[0], k)
idx_a = np.arange(labels.shape[0])
np.random.shuffle(idx_a)
k = 5000
idx = idx_a[:k]
dof = 2
state_i = labels[idx, :2 * dof]
state_f = labels[idx, 2 * dof:]
plt.subplot(1, 2, 1)
# Initial positions of the arm
x1 = np.cos(state_i[:, 0]) + np.cos(state_i[:, 1])
y1 = 1.0 * (np.sin(state_i[:, 0]) + np.sin(state_i[:, 1]))
omega = np.linalg.norm(state_i[:, 2:], axis=1)
c = np.linalg.norm(np.tile([-0.25 * np.pi, 0, 0, 0],
(state_i.shape[0], 1)) - state_i,
axis=1)
plt.scatter(x1, y1, s=2, alpha=0.8, c=c)
plt.colorbar().set_label('Euclidean distance to start pos')
plt.axis('equal')
plt.title('Initial position end effector')
# Start position
x, y = pol2cart(1, -0.25 * np.pi)
plt.plot([0, x, x + 1], [0, y, y], '-r', linewidth=2)
# Final positions of the arm
plt.subplot(1, 2, 2)
x2 = np.cos(state_f[:, 0]) + np.cos(state_f[:, 1])
y2 = 1.0 * (np.sin(state_f[:, 0]) + np.sin(state_f[:, 1]))
omega = np.linalg.norm(state_f[:, 2:], axis=1)
c = np.linalg.norm(np.tile([0.25 * np.pi, 0, 0, 0],
(state_f.shape[0], 1)) - state_f,
axis=1)
plt.scatter(x2, y2, s=2, alpha=0.8, c=c)
plt.colorbar().set_label('Euclidean distance to goal')
plt.axis('equal')
plt.title('Final position end effector')
# up up position
x, y = pol2cart(1, 0.25 * np.pi)
plt.plot([0, x, x + 1], [0, y, y], '-r', linewidth=2)
return None
def eef_pos_1dof(self):
"""
Display k=10000 configurations of the end effector
"""
plt.figure(figsize=(10, 6))
load.init_load("pendulum_time.csv", force=True)
_, labels = load.training_set()
k = 10000
idx = np.random.randint(0, labels.shape[0], k)
state_i = labels[idx, :2]
state_f = labels[idx, 2:]
plt.subplot(1, 2, 1)
# Initial positions of the arm
x1, y1 = pol2cart(1, state_i[:, 0])
omega = state_i[:, 1]
plt.scatter(x1, y1, s=2, alpha=0.8, c=omega)
plt.colorbar().set_label('Norm of omega')
plt.axis('equal')
plt.title('Initial position end effector')
# Down down position
x, y = pol2cart(1, -0.5 * np.pi)
plt.plot([0, x], [0, y], '-k', linewidth=2)
# Final positions of the arm
plt.subplot(1, 2, 2)
x2, y2 = pol2cart(1, state_f[:, 0])
omega = state_f[:, 1]
plt.scatter(x2, y2, s=2, alpha=0.8, c=omega)
plt.colorbar().set_label('Norm of omega')
plt.axis('equal')
plt.title('Final position end effector')
# up up position
x, y = pol2cart(1, 0.5 * np.pi)
plt.plot([0, x], [0, y], '-k', linewidth=2)
return None
self._saver.save(self._sess, path)
def restore_weights(self, path):
self._saver.restore(self._sess, path)
shutil.rmtree(self.logdir)
def close_session(self):
self._sess.close()
if __name__ == "__main__":
from data.loader import loader
gan = CLSGAN(6, 8, 32, {"g": [256, 256], "d": [256, 256]})
loader.init_load("2dof_time.csv")
batch_size = 100
train_dataset = tf.data.Dataset.from_tensor_slices(
loader.training_set()).repeat().batch(batch_size)
iterator = tf.data.Iterator.from_structure(train_dataset.output_types,
train_dataset.output_shapes)
train_init_op = iterator.make_initializer(train_dataset)
with gan.get_session() as sess:
sess.run(train_init_op)
training_data = iterator.get_next()
for it in range(30000):
X_mb, y_mb = sess.run(training_data)
Z_mb = gan.sample_z(batch_size)
for _ in range(1):
D_loss = gan.train_discriminator(Z_mb, X_mb, y_mb)
def cleaning(self):
loader.init_load()
train_data, train_labels = loader.training_set()
pass