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
