|
| 1 | +import tensorflow as tf |
| 2 | +import numpy as np |
| 3 | +from tensorflow.python.tools import freeze_graph |
| 4 | +import os |
| 5 | + |
| 6 | +input_size = 7 |
| 7 | +output_size = 3 |
| 8 | +batch_size = 500 |
| 9 | + |
| 10 | +learning_rate = 0.0001 |
| 11 | + |
| 12 | + |
| 13 | +def quat_vec_mul(rot, vec): |
| 14 | + a1 = rot[:,0] * 2 |
| 15 | + b2 = rot[:,1] * 2 |
| 16 | + c3 = rot[:,2] * 2 |
| 17 | + d4 = np.multiply(rot[:,0], a1) |
| 18 | + e5 = np.multiply(rot[:,1], b2) |
| 19 | + f6 = np.multiply(rot[:,2], c3) |
| 20 | + g7 = np.multiply(rot[:,0], b2) |
| 21 | + h8 = np.multiply(rot[:,0], c3) |
| 22 | + i9 = np.multiply(rot[:,1], c3) |
| 23 | + j10 = np.multiply(rot[:,3], a1) |
| 24 | + k11 = np.multiply(rot[:,3], b2) |
| 25 | + l12 = np.multiply(rot[:,3], c3) |
| 26 | + return np.transpose(np.stack(( |
| 27 | + np.multiply(1 - e5 - f6, vec[:,0]) + np.multiply(g7 - l12, vec[:,1]) + np.multiply(h8+k11, vec[:,2]), |
| 28 | + np.multiply(g7+l12, vec[:,0]) + np.multiply(1- d4-f6, vec[:,1]) + np.multiply(i9-j10, vec[:,2]), |
| 29 | + np.multiply(h8-k11, vec[:,0]) + np.multiply(i9+j10, vec[:,1]) + np.multiply(1-d4-e5, vec[:,2]) |
| 30 | + ))) |
| 31 | + |
| 32 | + |
| 33 | +def cross(vec1, vec2): |
| 34 | + x = vec1[:,0] |
| 35 | + y = vec1[:,1] |
| 36 | + z = vec1[:,2] |
| 37 | + a = vec2[:,0] |
| 38 | + b = vec2[:,1] |
| 39 | + c = vec2[:,2] |
| 40 | + return np.transpose(np.stack(( |
| 41 | + np.multiply(y,c) - np.multiply(z,b), |
| 42 | + np.multiply(z,a) - np.multiply(c,x), |
| 43 | + np.multiply(x,b) - np.multiply(a,y) |
| 44 | + ))) |
| 45 | + |
| 46 | +def dot(vec1, vec2): |
| 47 | + x = vec1[:,0] |
| 48 | + y = vec1[:,1] |
| 49 | + z = vec1[:,2] |
| 50 | + a = vec2[:,0] |
| 51 | + b = vec2[:,1] |
| 52 | + c = vec2[:,2] |
| 53 | + return np.transpose(np.expand_dims( |
| 54 | + np.multiply(x,a) + np.multiply(y,b) + np.multiply(z,c), axis = 0)) |
| 55 | + |
| 56 | +def normalize(vec): |
| 57 | + norm = dot(vec, vec)[:,0] |
| 58 | + vec[:,0] /= norm |
| 59 | + vec[:,1] /= norm |
| 60 | + vec[:,2] /= norm |
| 61 | + return vec |
| 62 | + |
| 63 | +assert(quat_vec_mul(np.array([[2,3,4,5]]), np.array([[6,7,8]]))[0,0] == -122) |
| 64 | +assert(quat_vec_mul(np.array([[2,3,4,5]]), np.array([[6,7,8]]))[0,1] == 71) |
| 65 | +assert(quat_vec_mul(np.array([[2,3,4,5]]), np.array([[6,7,8]]))[0,2] == 24) |
| 66 | + |
| 67 | +assert(cross(np.array([[2,3,4]]), np.array([[6,7,8]]))[0,0] == -4) |
| 68 | +assert(cross(np.array([[2,3,4]]), np.array([[6,7,8]]))[0,1] == 8) |
| 69 | +assert(cross(np.array([[2,3,4]]), np.array([[6,7,8]]))[0,2] == -4) |
| 70 | + |
| 71 | + |
| 72 | +forward = np.transpose(np.stack(( |
| 73 | + np.zeros(batch_size), |
| 74 | + np.zeros(batch_size), |
| 75 | + np.ones(batch_size) |
| 76 | + ))) |
| 77 | + |
| 78 | +up = np.transpose(np.stack(( |
| 79 | + np.zeros(batch_size), |
| 80 | + np.ones(batch_size), |
| 81 | + np.zeros(batch_size) |
| 82 | + ))) |
| 83 | + |
| 84 | +right = np.transpose(np.stack(( |
| 85 | + np.ones(batch_size), |
| 86 | + np.zeros(batch_size), |
| 87 | + np.zeros(batch_size) |
| 88 | + ))) |
| 89 | + |
| 90 | + |
| 91 | +graph = tf.Graph() |
| 92 | + |
| 93 | +with graph.as_default(): |
| 94 | + tensor_in = tf.placeholder(shape = [None, input_size], dtype=tf.float32, name='sensor') |
| 95 | + hidden1 = tf.layers.dense(tensor_in, 64, activation=tf.nn.elu) |
| 96 | + tensor_out = tf.layers.dense(hidden1, output_size, name='why_did_you_open_the_pandora_box', activation=tf.nn.elu) |
| 97 | + tf.identity(tensor_out, name='actuator') |
| 98 | + |
| 99 | + target = tf.placeholder(shape = [None, output_size], dtype=tf.float32, name='target') |
| 100 | + |
| 101 | + loss = tf.reduce_mean(tf.squared_difference(target, tensor_out)) |
| 102 | + optimizer = tf.train.AdamOptimizer(learning_rate=learning_rate) |
| 103 | + update_batch = optimizer.minimize(loss) |
| 104 | + |
| 105 | + saver = tf.train.Saver(max_to_keep=1) |
| 106 | + init = tf.global_variables_initializer() |
| 107 | + sess = tf.Session(graph=graph) |
| 108 | + sess.run(init) |
| 109 | + |
| 110 | + biased_in_data = None |
| 111 | + biased_target = None |
| 112 | + |
| 113 | + for epoch in range(100000): |
| 114 | + input_data = np.random.uniform(-1,1, |
| 115 | + size=(batch_size, input_size)) |
| 116 | + input_data[:,:3] = normalize(input_data[:,:3] ) |
| 117 | + pos = input_data[:,:3] |
| 118 | + rot = input_data[:,3:] |
| 119 | + fwd_rel_ref = quat_vec_mul(rot, forward) |
| 120 | + vec_cross = cross(pos, fwd_rel_ref) |
| 121 | + y_rot = dot(vec_cross, quat_vec_mul(rot, up)) |
| 122 | + x_rot = dot(vec_cross, quat_vec_mul(rot, right)) |
| 123 | + |
| 124 | + heuristic_target = np.transpose(np.stack(( |
| 125 | + np.clip(y_rot, -1, 1), |
| 126 | + np.clip(x_rot, -1, 1), |
| 127 | + (dot(vec_cross, vec_cross) < 1)*1.0 |
| 128 | + )))[0,:,:] |
| 129 | + |
| 130 | + firing_data_index = (heuristic_target[:, 2] > 0.5) |
| 131 | + |
| 132 | + for i in range(5): |
| 133 | + l, _ = sess.run((loss, update_batch), feed_dict = { |
| 134 | + tensor_in: input_data, |
| 135 | + target: heuristic_target |
| 136 | + }) |
| 137 | + |
| 138 | + if epoch % 100 == 0: |
| 139 | + print(epoch, sum(firing_data_index), l) |
| 140 | + |
| 141 | + |
| 142 | + saver.save(sess, './last_checkpoint.ckpt') |
| 143 | + tf.train.write_graph(graph, '.', 'raw_graph_def.pb', as_text=False) |
| 144 | + print([x.name for x in graph.as_graph_def().node]) |
| 145 | + freeze_graph.freeze_graph( |
| 146 | + input_graph='./raw_graph_def.pb', |
| 147 | + input_binary=True, |
| 148 | + input_checkpoint='./last_checkpoint.ckpt', |
| 149 | + output_node_names='actuator', |
| 150 | + output_graph=('test.bytes'), |
| 151 | + clear_devices=True, initializer_nodes='', |
| 152 | + input_saver='', |
| 153 | + restore_op_name='save/restore_all', |
| 154 | + filename_tensor_name='save/Const:0') |
| 155 | + |
| 156 | + |
0 commit comments