with open('training.csv', 'w') as training:
writer = csv.writer(training)
writer.writerow([
'theta0', 'theta1', 'theta2',
'phi0', 'phi1', 'phi2',
'centroid_x', 'centroid_y', 'centroid_z',
'normal_x', 'normal_y', 'normal_z',
'yaw', 'pitch'
])
i = 0
while i < 100:
try:
theta = np.random.rand(3) - 0.5
phi = km.solve_phi(theta)
c, centroid, normal = km.state_for(theta, phi)
row = np.hstack((
theta,
phi.T,
np.array(centroid),
normal.T,
np.array([yaw, pitch])
))
writer.writerow(row)
i = i + 1
print("{0} training points".format(i))
except:
pass
theta = np.apply_along_axis(motion, 1, theta)
plt.subplot(211)
plt.plot(t_domain, theta[:, 0], label="theta0")
plt.plot(t_domain, theta[:, 1], label="theta1")
plt.plot(t_domain, theta[:, 2], label="theta2")
plt.legend()
platform = np.empty([0, 3])
info = np.array([]).reshape(0, 3)
for i, row in enumerate(theta):
print "Solving {0} of {1}".format(i, len(theta))
phi = km.solve_phi(row)
a, b, c, centroid, normal, yaw, pitch = km.state_for(row, phi)
normal_x = normal[0]
normal_y = normal[2]
z = centroid[2]
info = np.vstack([info, np.array([normal_x, normal_y, z])])
print info
plt.subplot(212)
plt.plot(t_domain, info[:, 0], label="normal x")
plt.plot(t_domain, info[:, 1], label="normal y")
plt.plot(t_domain, info[:, 2], label="centroid z")
plt.legend()
