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.vstack([t_domain, t_domain, t_domain]).T 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()