def p_punt_function_call_end(p): ''' punt_function_call_end : empty ''' global quadruples global quadCont global funcName global scopeTable global variables global operators global actualScope global recursiveCalls if len(operators) > 0: operators.pop() dir = scopeTable.scopes[funcName][2] quadruples.append(Quad('GoSub', None, None, dir)) quadCont += 1 if scopeTable.scopes[funcName][0] != 'void': returnval = scopeTable.scopes[funcName][3] if funcName == actualScope: direction = calc_new_direction(scopeTable.scopes[funcName][0], actualScope) variables.append(direction) quadruples.append(Quad('SetReturnValue', None, None, direction)) recursiveCalls.append(quadCont) quadCont += 1 else: direction = calc_new_direction(scopeTable.scopes[actualScope][0], actualScope) quadruples.append(Quad('SetReturnValue', None, None, direction)) variables.append(direction) quadCont += 1 p[0] = p[1]
def p_loop_value(p): ''' loop_value : hyper_exp ''' global variables global conditions global quadruples global quadCont global jumps global ranges up = pop_from_variables(p) low = ranges[-1] operator = conditions.pop() result = valid_operation(operator, low, up) if result == -1: print("Invalid operation at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad(operator, up, low, result)) quadCont += 1 variables.append(result) jumps.append(quadCont) bool = pop_from_variables(p) quadruples.append(Quad('GoToF', None, bool, None)) quadCont += 1 p[0] = p[1]
def p_patron(p): ''' patron : patron_A hyper_exp ''' global quadruples global variables global quadCont global ranges global patrons global tempCont global scopeTable up = pop_from_variables(p) low = ranges[-1] operator = pop_from_patrons(p) result = valid_operation(operator, low, up) if result == -1: print("Invalid operation at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad(operator, up, low, result)) variables.append(result) quadCont += 1 value = pop_from_variables(p) control_var = pop_from_ranges(p) quadruples.append(Quad('=', None, value, control_var)) quadCont += 1 returning = jumps.pop() goto = jumps.pop() quadruples.append(Quad('GoTo', None, None, goto)) quadCont += 1 quadruples[returning].result = quadCont p[0] = "" for x in range(1, len(p)): p[0] += str(p[x]) p[0]
def p_function_D(p): # Return value for function ''' function_D : WITH hyper_exp | empty ''' global actualScope global scopeTable global quadruples global quadCont global funcType if funcType != 'void': result = pop_from_variables(p) result_type = get_type_by_direction(result) if funcType == result_type: quadruples.append(Quad('return',None,None,result)) scopeTable.scopes[actualScope][3] = result else: print("Type mismatch at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad('return',None,None,None)) quadCont += 1 quadruples.append(Quad('EndProc',None,None,None)) quadCont += 1 p[0] = "" for x in range(1, len(p)): p[0] += str(p[x]) p[0]
def __init__(self, *args, **kwargs): self.boundary = '--boundarydonotcross' self.html = open('index.html', 'r').read() # Handle Arguments self.input_path = _args.i[0] self.rotation = _args.r[0] self.output_width = int(_args.c[0].split('x')[0]) self.output_height = int(_args.c[0].split('x')[1]) self.frame_rate = _args.f[0] self.frame_skip_rate = _args.k[0] self.recognize_scale = 0.2 self.predictor = dlib.shape_predictor('../shapes/shape_predictor_68_face_landmarks.dat') self.face_cascade = toolbox.loadCascade('haarcascade_frontalface_default.xml') # Define skipping vars self.skip_frames = None self.skip_frame_x = None self.skip_frame_y = None self.skip_rect = None self.skip_points = None # OpenGL self.quad = Quad() self.quad_program = ShaderProgram(fragment=self.quad.fragment, vertex=self.quad.vertex) self.quad.loadVBOs(self.quad_program) self.quad.loadElements() super(CustomHandler, self).__init__(*args, **kwargs)
def simulate_with_actions(state, N, action_schedule): quad_dev = Quad(state) trajectory = [state.copy()] action_schedule[:, 2] += M * G for i in range(N-1): action = action_schedule[i] state, _ = quad_dev.step(state, action) trajectory.append(state.copy()) return np.array(trajectory)
def p_factor(p): ''' factor : minus value | OPEN_PARENTHESIS puntOP hyper_exp CLOSE_PARENTHESIS puntCP ''' global variables global scopeTable global actualScope global actual_value global quadCont global quadruples global operators global negativeflag if p[1] != '(' and check_if_exist(p[2]): variables.append(actual_value) if len(operators) > 0: if operators[-1] == '-' and negativeflag ==1: oper = pop_from_operators(p) var1 = pop_from_variables(p) result = valid_operation(oper, var1, var1) if result == -1: print("Invalid operation at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad(oper,var1,0,result)) variables.append(result) quadCont += 1 flag = 0 p[0] = "" for x in range(1, len(p)): p[0] += str(p[x]) p[0]
def quad_object(self, f=None, fvals=None, ufunc=True): """ Return an object with the Quad interface interface expected by Composite quadtrature objects, using a specified f() (or a set of its values on the fnodes) to define a quadrature rule for g(). """ if f is not None or fvals is not None: # otherwise assume prev. set self.set_f(f, fvals, ufunc) quad = Quad(self.a, self.b, self.g_nodes, self.g_wts) # Add an attribute pointing back to this PolyQuadRule instance. quad.creator = self # Add a range methods corresponding to quad_g_range and get_g_nodes_wts, # for use in composite rules. quad.quad_range = self.quad_g_range quad.quad_range_nodes_wts = self.get_g_nodes_wts return quad
def p_punt_start_litaf(p): ''' punt_start_litaf : empty ''' global quadCont global quadruples create_scope('constants', 'void', quadCont, p) quadruples.append(Quad('GoTo',None,None,None)) quadCont += 1 p[0] = p[1]
def _select_instructions(self): if self._backend_name == 'quad': from quad import Quad as backend else: raise self.Error(f'Unsupported back-end \'{self._backend_name}\'') for bb in self._basic_blocks: backend_instrs = [] for ir_instr in bb.instructions: backend_instrs += backend.map_instruction(ir_instr) bb.instructions = backend_instrs
def initGL(output_width, output_height): glutInit(sys.argv) glutInitDisplayMode(GLUT_3_2_CORE_PROFILE | GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH) glutInitWindowSize(output_width, output_height) glutCreateWindow("Virtual Window") print('') print('Vendor: ' + glGetString(GL_VENDOR).decode('utf-8')) print('Renderer: ' + glGetString(GL_RENDERER).decode('utf-8')) print('OpenGL Version: ' + glGetString(GL_VERSION).decode('utf-8')) print('Shader Version: ' + glGetString(GL_SHADING_LANGUAGE_VERSION).decode('utf-8')) if not glUseProgram: print('Missing Shader Objects!') sys.exit(1) glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR) glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR) global QUAD QUAD = Quad() global QUAD_PROGRAM QUAD_PROGRAM = ShaderProgram(fragment=QUAD.fragment, vertex=QUAD.vertex) QUAD.loadVBOs(QUAD_PROGRAM) QUAD.loadElements()
def p_puntElseIfGoToF(p): ''' puntElseIfGoToF : empty ''' global quadruples global variables global quadCont global jumps jumps.append(quadCont) bool = pop_from_variables(p) quadruples.append(Quad('GoToF',None, bool, None)) quadCont += 1 p[0] = p[1]
def p_puntUntil(p): ''' puntUntil : empty ''' global variables global quadruples global quadCont global jumps result = pop_from_variables(p) quadruples.append(Quad('GoToF', None, result, None)) jumps.append(quadCont) quadCont += 1 p[0] = p[1]
def p_puntUntilEnd(p): ''' puntUntilEnd : empty ''' global quadruples global quadCont global jumps end = jumps.pop() # Pops GOTOF QUAD and fills the missing jump with actual counter quadruple quadCont += 1 quadruples[end].result = quadCont returning = jumps.pop() # Pops QUAD for generating GOTO QUAD to re evaluation of the conditional exp of the cycle quadruples.append(Quad('GoTo', None, None, returning)) p[0] = p[1]
def p_function_call_hyper_exp(p): ''' function_call_hyper_exp : punt_false_bottom_function hyper_exp ''' global quadCont global contParams global quadruples global variables global operators contParams += 1 param = pop_from_variables(p) quadruples.append(Quad('PARAM', None, param, 'param'+str(contParams))) quadCont += 1 p[0] = p[1]
def p_puntElse(p): ''' puntElse : empty ''' global quadruples global variables global quadCont global jumps quadruples.append(Quad('GoTo',None,None,None)) false = jumps.pop() jumps.append(quadCont) quadCont += 1 quadruples[false].result = quadCont p[0] = p[1]
def p_puntElseIfGOTO(p): ''' puntElseIfGOTO : empty ''' global quadruples global variables global quadCont global jumps returning = jumps.pop() jumps.append(quadCont) quadruples.append(Quad('GoTo', None, None, None)) quadCont += 1 quadruples[returning].result = quadCont p[0] = p[1]
def p_lecture_ID(p): ''' lecture_ID : ID ''' global quadruples global quadCont global actual_value if check_if_exist(p[1]): lec = actual_value quadruples.append(Quad('Lecture', None, None, lec)) quadCont += 1 else: print("Syntax error at line {}: undeclared variable '{}'".format(p.lexer.lineno, p[1])) sys.exit(0) p[0] = p[1]
def p_writing_A(p): ''' writing_A : hyper_exp writing_A1 ''' global variables global writCont global output writCont = 0 message = pop_from_variables(p) output.append(Quad('Writing', None, None, message)) writCont += 1 p[0] = "" for x in range(1, len(p)): p[0] += str(p[x]) p[0]
def p_function_call_name(p): ''' function_call_name : FUNCTION_ID ''' global funcName global quadruples global scopeTable global funcName global block_flag global quadCont funcName = p[1] type = scopeTable.scopes[funcName][0] quadruples.append(Quad('ERA', None, None, funcName)) quadCont += 1 p[0] = p[1]
def __init__(self, Stable, wm, quad_pos=(0, 0, 0)): self.world = World() self.world.setGravity((0, -9.81, 0)) self.space = Space() self.contactgroup = JointGroup() self.wm = wm # draw floor self.floor = GeomPlane(self.space, (0, 1, 0), -1) box(pos=(0, -1, 0), width=2, height=0.01, length=2) # Draw Quad self.quad = Quad(self.world, self.space, pos=quad_pos) # Init Algorithm self.stable = Stable(self.quad) # stop autoscale of the camera scene.autoscale = False
def p_bool_values_cycle(p): ''' bool_values_cycle : bool_values ''' global quadruples global quadCont global variables global tempCont up = pop_from_variables(p) low = scopeTable.scopes['constants'][1].vars[p[1]][1] operator = "==" result = valid_operation(operator, low, up) if result == -1: print("Invalid operation at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad(operator, up, low, result)) variables.append(result) quadCont += 1 p[0] = p[1]
def save_animation(self, animation_name): if animation_name != '': filename = os.path.join(self.dir, 'animations/') anim_file = open(filename + animation_name + '.txt', 'w') l_animation_queue = [] r_animation_queue = [] l_gripper_queue = [] r_gripper_queue = [] for i in range(self.list_widget.count()): item = self.list_widget.item(i) pose_name = item.text() anim_file.write( re.sub( ',', '', str(self.animation_map[pose_name].left).strip('[]') + ' ' + str(self.animation_map[pose_name].right).strip('[]'))) anim_file.write('\n') anim_file.write( str(self.animation_map[pose_name].left_gripper) + ' ' + str(self.animation_map[pose_name].right_gripper)) l_animation_queue.append(self.animation_map[pose_name].left) r_animation_queue.append(self.animation_map[pose_name].right) l_gripper_queue.append( self.animation_map[pose_name].left_gripper) r_gripper_queue.append( self.animation_map[pose_name].right_gripper) anim_file.write('\n') anim_file.close() self.saved_animations[animation_name] = Quad( l_animation_queue, r_animation_queue, l_gripper_queue, r_gripper_queue) self.saved_animations_list.addItem( animation_name) # Bug? Multiple entries # Reset the pending queue self.l_animation_queue = [] self.r_animation_queue = [] else: self.show_warning('Please insert name for animation')
def extraction_algorithm(fine_data, resolutions, dimensions): print "### Dual Contouring ###" [verts_out_dc, quads_out_dc, manifolds, datasets] = tworesolution_dual_contour(fine_data, resolutions, dimensions) print "### Dual Contouring DONE ###" N_quads = { 'coarse': quads_out_dc['coarse'].__len__(), 'fine': quads_out_dc['fine'].__len__() } N_verts = { 'coarse': verts_out_dc['coarse'].__len__(), 'fine': verts_out_dc['fine'].__len__() } quads = { 'coarse': [None] * N_quads['coarse'], 'fine': quads_out_dc['fine'] } verts = { 'coarse': verts_out_dc['coarse'], 'fine': verts_out_dc['fine'] } # todo substitute with vertex objects for i in range(N_quads['coarse']): quads['coarse'][i] = Quad(i, quads_out_dc['coarse'], verts_out_dc['coarse']) assert quads['coarse'].__len__() > 0 print "### Projecting Datapoints onto coarse quads ###" # do projection of fine verts on coarse quads param = create_parameters(verts, quads) #export_results.export_as_csv(param,'./PYTHON/NURBSReconstruction/DualContouring/plotting/param') print "### Projecting Datapoints onto coarse quads DONE ###" return verts_out_dc, quads_out_dc, quads, param, datasets
def p_puntSum(p): ''' puntSum : empty ''' global operators global quadruples global quadCont global variables if len(operators) > 0: if operators[-1] == '+' or operators[-1] == '-': operator = pop_from_operators(p) operand2 = pop_from_variables(p) operand1 = pop_from_variables(p) result = valid_operation(operator, operand1, operand2) if result == -1: print("Invalid operation at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad(operator, operand2, operand1, result)) variables.append(result) quadCont += 1 p[0] = p[1]
def p_assign(p): ''' assign : ID EQUAL appendEqual assign_A ''' global operators global quadruples global variables global quadCont global actual_value operator = pop_from_operators(p) operand2 = pop_from_variables(p) if check_if_exist(p[1]): operand1 = actual_value result = valid_operation(operator, operand1, operand2) if result == -1: print("Invalid operation at {}".format(p.lexer.lineno)) sys.exit(0) else: quadruples.append(Quad(operator, None, operand2, operand1)) quadCont += 1 p[0] = "" for x in range(1, len(p)): p[0] += str(p[x]) p[0]
def main(): simulation_time = 20 ## prepare the trajectory(course) to track idx = np.arange(0, 4 * simulation_time, dl) course_pos = curve_pos(idx) course_vel = curve_vel(idx) speed = 0.5 # m/s course_vel = course_vel / np.linalg.norm(course_vel, axis=0) * (speed + np.random.rand()) # normalize with the same speed, but not direction. course_vel[:, 0] = 0 # avoid nan course_vel[:, -1] = 0 # stop course = np.concatenate((course_pos, course_vel), axis=0) # desired state with dim 6, shape [dim, timeslot] # init state and target state init_state = course[:, 0] + np.array([1., 2., 0., 0., 0., 0.])#np.array([-1., 1., 0., 0., 0., 0.]) # state = init_state nearest, _ = calc_nearest_index(state, course, 0) target_states = course[:, nearest] # quadrotor object quad = Quad(init_state) # dimension nu = 4 nx = init_state.shape[0] # 6 # controller parameters setting for both lqr and linear mpc u = np.zeros(nu) horizon = 20 # predict and control horizon in mpc Q = np.array([20. , 20., 20., 8, 8, 8]) QN = Q # Q = np.array([8. , 8., 8., 0.8, 0.8, 0.8]) # QN = np.array([10., 10., 10., 1.0, 1.0, 1.0]) R = np.array([6., 6., 6.]) Ad, Bd = linearized_model(state) # actually use linear time-invariant model linearized from the equilibrium point # record state labels = ['x', 'y', 'z', 'vx', 'vy', 'vz'] # convenient for plot state_control = {x: [] for x in labels} target_chosen = {x: [] for x in labels} for ii in range(6): target_chosen[labels[ii]].append(target_states[ii]) err = [] # tracking error # visualization fig = plt.figure() ax = plt.axes(projection='3d') # control loop timestamp = 0.0 while timestamp <= simulation_time: print("timestamp", timestamp) if TRACKING: target_states, nearest, chosen_idxs = calc_ref_trajectory(state, course, nearest, horizon) else: target_states = np.array([0, 0, 10, 0, 0, 0]) if CONTROLLER=='MPC': mpc_policy = mpc(state, target_states, Ad, Bd, horizon, Q, QN, R, TRACKING) nominal_action, actions, nominal_state = mpc_policy.solve() action = nominal_action #- np.dot(K,(target_states - nominal_state)) roll_r, pitch_r, thrust_r = action u[0] = - pitch_r u[1] = - roll_r u[2] = 0.0 u[3] = thrust_r + M * G elif CONTROLLER=='LQR': lqr_policy = FiniteHorizonLQR(Ad, Bd, Q, R, QN, horizon=horizon) # instantiate a lqr controller lqr_policy.set_target_state(target_states) ## set target state to koopman observable state lqr_policy.sat_val = np.array([PI/8., PI/8., 0.75]) K, ustar = lqr_policy(state) u[0] = - ustar[1] u[1] = - ustar[0] u[2] = 0.0 u[3] = ustar[2] + M * G print("K", K) elif CONTROLLER=='PID': # Compute control errors x, y, z, dx, dy, dz = state x_r, y_r, z_r, dx_r, dy_r, dz_r = target_states ex = x - x_r ey = y - y_r ez = z - z_r dex = dx - dx_r dey = dy - dy_r dez = dz - dz_r xi = 1.2 wn = 3.0 Kp = - wn * wn Kd = - 2 * wn * xi Kxp = 1.2 * Kp Kxd = 1.2 * Kd Kyp = Kp Kyd = Kd Kzp = 0.8 * Kp Kzd = 0.8 * Kd pitch_r = Kxp * ex + Kxd * dex roll_r = Kyp * ey + Kyd * dey thrust_r = (Kzp * ez + Kzd * dez + 9.8) * 0.44 u[0] = pitch_r u[1] = roll_r u[2] = 0. u[3] = thrust_r + M * G next_state = quad.step(state, u) state = next_state timestamp = timestamp + dt if TRACKING: err.append(state[:3] - target_states[:3, -1]) else: err.append(state[:3] - target_states[:3]) # record for ii in range(len(labels)): state_control[labels[ii]].append(state[ii]) if TRACKING: target_chosen[labels[ii]].append(target_states[ii, -1]) else: target_chosen[labels[ii]].append(target_states[ii]) ## plot if True: #timestamp > dt: # # plot in real time plt.cla() x = np.append(init_state[0], state_control['x']) y = np.append(init_state[1], state_control['y']) z = np.append(init_state[2], state_control['z']) final_idx = len(x) - 1 ax.plot3D(x, y, z, label='Quadcopter flight trajectory') ax.plot3D([init_state[0]], [init_state[1]], [init_state[2]], label='Initial position', color='blue', marker='x') ax.plot3D([x[final_idx]], [y[final_idx]], [z[final_idx]], label='Final position', color='green', marker='o') if TRACKING: plt_len = chosen_idxs[-1] + 100 ax.plot3D(course[0, :plt_len], course[1, :plt_len], course[2, :plt_len]) # ax.scatter3D([course[0, chosen_idxs[0]]], [course[1, chosen_idxs[0]]], [course[2, chosen_idxs[0]]], color='red', label='chosen target flight trajectory', # marker='o') for i in range(horizon): ax.scatter3D([course[0, chosen_idxs[i]]], [course[1, chosen_idxs[i]]], [course[2, chosen_idxs[i]]], color='red', marker='o') ax.scatter3D([target_states[0]], [target_states[1]], [target_states[2]], color='red', marker='o') plt.legend() plt.pause(0.01) err = np.stack(err) plt.figure() plt.plot(err[:,0], label='x') plt.plot(err[:,1], label='y') plt.plot(err[:,2], label='z') plt.legend() plt.show()
def main(): parser = argparse.ArgumentParser() parser.add_argument("--epoch_size", type=int, default=10, help="Total training epochs") parser.add_argument("--episode_size", type=int, default=50000, help="Training episodes per epoch") parser.add_argument("--epsilon", type=float, default=0.1, help="Greedy Epsilon starting value") parser.add_argument("--gamma", type=float, default=0.99, help="DQN gamma starting value") parser.add_argument("--replay_buffer", type=float, default=100, help="Replay buffer size") parser.add_argument("--load_model", action='store_true', default=False, help="Load saved model") parser.add_argument("--test", action='store_true', default=False, help="Testing phase") parser.add_argument("--nodisplay", action='store_true', default=False, help="Show V-REP display") parser.add_argument("--cuda", action='store_true', default=False, help="Use CUDA") parser.add_argument("--viz", action='store_true', default=False, help="Use Visdom Visualizer") args = parser.parse_args() print("Using Parameters:") print("Epoch Size: %d" % args.epoch_size) print("Episode Size: %d" % args.episode_size) print("Epsilon: %f" % args.epsilon) print("Gamma: %f" % args.gamma) print("Replay buffer size: %f" % args.replay_buffer) print("Testing Phase: %s" % str(args.test)) print("Using CUDA: %s\n" % str(args.cuda)) # Initialize classes control_quad = QuadHelper() dqn_quad = QuadDQN(args.cuda, args.epoch_size, args.episode_size) if args.viz: viz = Visualizer() main_quad = Quad(dqn_quad=dqn_quad, control_quad=control_quad, visualizer=viz) else: main_quad = Quad(dqn_quad=dqn_quad, control_quad=control_quad, visualizer=None) # Argument parsing dqn_quad.buffer_size = args.replay_buffer dqn_quad.eps = args.epsilon dqn_quad.gamma = args.gamma if args.load_model: dqn_quad.load_wts('dqn_quad.pth') if args.nodisplay: control_quad.display_disabled = True main_quad.display_disabled = True while (vrep.simxGetConnectionId(control_quad.sim_quad.clientID) != -1): with open('dqn_outputs.txt', 'a') as the_file: log_time = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') the_file.write('\n************* SAVE FILE %s *************\n' % log_time) if args.test: print("Testing Quadrotor") # Test our trained quadrotor main_quad.mode = 'test' dqn_quad.load_wts('dqn_quad.pth') main_quad.test_quad() else: # Train quadcopter epoch = 0 while epoch < dqn_quad.epoch_size: main_quad.run_one_epoch(epoch) print("Epoch reset") epoch += 1 main_quad.task_every_n_epochs(epoch) print('\n') print("Finished training") # Test quadrotor main_quad.mode = "test" main_quad.test_quad() control_quad.sim_quad.exit_sim() print("V-REP Exited...")
class CustomHandler(BaseHTTPRequestHandler, object): def __init__(self, *args, **kwargs): self.boundary = '--boundarydonotcross' self.html = open('index.html', 'r').read() # Handle Arguments self.input_path = _args.i[0] self.rotation = _args.r[0] self.output_width = int(_args.c[0].split('x')[0]) self.output_height = int(_args.c[0].split('x')[1]) self.frame_rate = _args.f[0] self.frame_skip_rate = _args.k[0] self.recognize_scale = 0.2 self.predictor = dlib.shape_predictor('../shapes/shape_predictor_68_face_landmarks.dat') self.face_cascade = toolbox.loadCascade('haarcascade_frontalface_default.xml') # Define skipping vars self.skip_frames = None self.skip_frame_x = None self.skip_frame_y = None self.skip_rect = None self.skip_points = None # OpenGL self.quad = Quad() self.quad_program = ShaderProgram(fragment=self.quad.fragment, vertex=self.quad.vertex) self.quad.loadVBOs(self.quad_program) self.quad.loadElements() super(CustomHandler, self).__init__(*args, **kwargs) def do_GET(self): self.send_response(200) if self.path.endswith('.mjpg'): # Response headers (multipart) self.send_header('Cache-Control', 'no-store, no-cache, must-revalidate, pre-check=0, post-check=0, max-age=0') self.send_header('Connection', 'close') self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=%s' % self.boundary) self.send_header('Expires', 'Mon, 3 Jan 2000 12:34:56 GMT') self.send_header('Pragma', 'no-cache') stream = self.init_connection(self.input_path) stream_bytes = b'' for line in stream.iter_content(chunk_size=2048, decode_unicode=False): stream_bytes += line a = stream_bytes.find(b'\xff\xd8') # Start of a frame b = stream_bytes.find(b'\xff\xd9') # End of a frame if a != -1 and b != -1: frame_bytes = stream_bytes[a:b+2] stream_bytes = stream_bytes[b+2:] frame = cv2.imdecode(np.fromstring(frame_bytes, dtype=np.uint8), cv2.IMREAD_COLOR) frame, points = self.processFrame(frame, self.rotation, self.frame_skip_rate, self.face_cascade, self.predictor, self.recognize_scale, self.output_width, self.output_height) if frame is not None: # if points is not None: # frame = self.getGLFrame(frame, points, self.output_width, self.output_height) jpg = cv2.imencode('.jpg', frame)[1] # Part boundary string self.end_headers() self.wfile.write(bytes(self.boundary.encode('utf-8'))) self.end_headers() # Part headers self.send_header('X-Timestamp', time.time()) self.send_header('Content-length', str(len(jpg))) self.send_header('Content-type', 'image/jpeg') self.end_headers() # Write Binary self.wfile.write(bytes(jpg)) else: self.send_header('Content-type', 'text/html') self.end_headers() self.wfile.write(bytes(self.html.encode('utf-8'))) def init_connection(self, url): session = requests.Session() request = requests.Request("GET", url).prepare() response_stream = session.send(request, stream=True) return response_stream def log_message(self, format, *args): return def rotateFrame(self, image, angle, crop=True): if (angle < 0): angle = 360 + angle if (angle == 0): return image if (angle != 90 and angle != 180 and angle != 270): raise NameError('You can only rotate the image in steps of 90 / -90 degree') return image if (angle == 180): (h, w) = image.shape[:2] center = (w / 2, h / 2) matrix = cv2.getRotationMatrix2D(center, -angle, 1.0) result = cv2.warpAffine(frame, matrix, (w, h)) return result (h, w) = image.shape[:2] size = max(w, h) canvas = np.zeros((size, size, 3), np.uint8) x = int((size - w) / 2) y = int((size - h) / 2) canvas[y:y+h, x:x+w] = image center = (size / 2, size / 2) matrix = cv2.getRotationMatrix2D(center, -angle, 1.0) canvas = cv2.warpAffine(canvas, matrix, (size, size)) if (crop): canvas = canvas[x:x+w, y:y+h] return canvas def cropFrame(self, frame, x, y, w, h): rows, cols = frame.shape[:2] if (cols > w and rows > h): return frame[y:y+h, x:x+w] else: return frame def getGLFrame(self, frame, points, output_width, output_height): p1, p2, p3 = points[39], points[42], points[33] w, h = output_width, output_height p1 = [(p1[1] / w) * 2 - 1, -((p1[0] / h) * 2 - 1)] p2 = [(p2[1] / w) * 2 - 1, -((p2[0] / h) * 2 - 1)] p3 = [(p3[1] / w) * 2 - 1, -((p3[0] / h) * 2 - 1)] triangle = Triangle(p1, p2, p3) tri_program = ShaderProgram(fragment=triangle.fragment, vertex=triangle.vertex) triangle.loadVBOs(tri_program) triangle.loadElements() glClear(GL_COLOR_BUFFER_BIT) glClearColor (0.0, 0.0, 0.0, 1.0) #-----------------------------------# self.quad_program.start() toolbox.bind(self.quad) glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, w, h, 0, GL_BGR, GL_UNSIGNED_BYTE, frame) glDrawElements(GL_TRIANGLES, len(self.quad.elements) * 3, GL_UNSIGNED_INT, None) toolbox.unbind() self.quad_program.stop() #-----------------------------------# tri_program.start() toolbox.bind(triangle) glDrawElements(GL_TRIANGLES, len(triangle.elements) * 3, GL_UNSIGNED_INT, None) toolbox.unbind() tri_program.stop() #-----------------------------------# glFinish() glPixelStorei(GL_PACK_ALIGNMENT, 1) buffer = glReadPixels(0, 0, output_width, output_height, GL_BGR, GL_UNSIGNED_BYTE) image = Image.frombytes('RGB', (output_width, output_height), buffer) image = image.transpose(Image.FLIP_TOP_BOTTOM) frame = np.asarray(image, dtype=np.uint8) glutSwapBuffers() return frame def processFrame(self, frame, rotation, frame_skip_rate, face_cascade, predictor, recognize_scale, output_width, output_height): points = None # Roate the frame # frame = self.rotateFrame(frame, rotation) return (frame, points) scale = (1 / recognize_scale) # Check how many frames have been skipped if self.skip_frames is not None and self.skip_frames > 0: self.skip_frames -= 1 if self.skip_frame_x is not None and self.skip_frame_y is not None: frame = self.cropFrame(frame, self.skip_frame_x, self.skip_frame_y, output_width, output_height) points = self.skip_points else: self.skip_frames = frame_skip_rate # Get current frame width and height for later usage (frame_height, frame_width) = frame.shape[:2] # Create a low resolution version of the rotated frame small = cv2.resize(frame, (0,0), fx=recognize_scale, fy=recognize_scale) gray = cv2.cvtColor(small, cv2.COLOR_BGR2GRAY) # Recognize a face on the low reslution frame faces = face_cascade.detectMultiScale(gray, 1.1, 5) for (x, y, w, h) in faces: # Scale up coordinates x, y, w, h = int(x * scale), int(y * scale), int(w * scale), int(h * scale) # Crop the frame frame_x = int((frame_width - output_width) / 2) frame_y = y - int((output_height - h) / 2) frame = self.cropFrame(frame, frame_x, frame_y, output_width, output_height) # Normalize coordinates to the cropped frame x = x - frame_x y = y - frame_y # Create a low resolution version of the cropped frame small = cv2.resize(frame, (0,0), fx=recognize_scale, fy=recognize_scale) # Find all the landmarks on the face rs = recognize_scale low_rect = (x * rs, y * rs, w * rs, h * rs) shape = predictor(small, toolbox.rect2rectangle(low_rect)) points = np.array([[p.y * scale, p.x * scale] for p in shape.parts()]) toolbox.drawTriangles(frame, points) # Save values to use while skipping frames self.skip_frame_x = frame_x self.skip_frame_y = frame_y self.skip_rect = (x, y, w, h) self.skip_points = points return (frame, points)
mymotor1 = motor('br', 17, debug=False, simulation=False) # RL mymotor2 = motor('fr', 18, debug=False, simulation=False) # RR mymotor3 = motor('bl', 27, debug=False, simulation=False) # FR mymotor4 = motor('fl', 22, debug=False, simulation=False) # FL mymotor1.setMaxSpeed(80) mymotor2.setMaxSpeed(80) mymotor3.setMaxSpeed(80) mymotor4.setMaxSpeed(80) mymotor1.setMinSpeed(5) mymotor2.setMinSpeed(5) mymotor3.setMinSpeed(5) mymotor4.setMinSpeed(5) quadcopter = Quad(mymotor1, mymotor2, mymotor3, mymotor4, imu_controller) quadcopter.dec_height(5) print 'Setting zero' quadcopter.load_zero() print quadcopter.zero_x,quadcopter.zero_y,quadcopter.zero_z time.sleep(2) quadcopter.balancer() print 'Zero set' pr=0 p=0 i=0 d=0 # quadcopter.set_PID(p, i, d, pr) quadcopter.set_trims(0, 7, 0, False) success = 5
from quad import Quad from time import sleep if __name__ == "__main__": q = Quad("quad.config") q[0].set(150, 150, 0) sleep(10)
from semanticCube import SemanticCube from symbolTable import SymbolTable, Variable from quad import Quad import condHelpers import helpers quadruple = Quad.instantiate() symbolTable = SymbolTable.instantiate() varPointer = None tempCounter = 0 # getPointingScope # What: Gets the pointer to either global or class memory # Parameters: The current Scope (function or class) # Returns the Scope from the sent function or class # When is it used: To get the Scope for the quad to be saved def getPointingScope(currentScope): if currentScope.getContext() == 'classFunction': return symbolTable.getGlobalScope().getScopeClasses()[symbolTable.getStack()] else: return symbolTable.getGlobalScope() # check_multdiv_operator # What: Gets and checks left and right operators for multiplication and division # Parameters: A quadruple object # When is it used: When PLY reads a multiplication or division def check_multdiv_operator(quadruple): workingStack = quadruple.getWorkingStack() if workingStack: if workingStack[-1] == '*' or workingStack[-1] == '/':
def __init__(self): Quad.__init__(self) self.rect.left = 500 self.ticks = 0 self.move_tick = 0 self.directionLR = self.NEUTRAL