def keyboardControl(self):
self.count = 0
app = drawAction.drawAction()
self.screen.fill((255, 255, 255))
while True:
currentPos = app.motionProxy.getPosition("RHand", motion.FRAME_TORSO, False)
currentPos = almath.Position6D(currentPos)
# Change coordiante from torso to right shoulder joint
self.printText("Press 'Esc' to exit.", "Times New Roman", 20, 10, 10, (255, 0, 0))
self.printText("X Coordinate: " + str(round(currentPos.x, 3)), "Times New Roman", 15, 10, 40, (255, 0, 0))
self.printText("Y Coordinate: " + str(round(currentPos.y + 0.098, 3)), "Times New Roman", 15, 10, 65,(255, 0, 0))
self.printText("Z Coordinate: " + str(round(currentPos.z - 0.1, 3)), "Times New Roman", 15, 10, 90,(255, 0, 0))
# max fps limit
self.clock.tick(30)
for event in pygame.event.get():
if event.type == pygame.QUIT:
return
elif event.type == pygame.KEYDOWN:
# press esc to clear screen
if event.key == pygame.K_ESCAPE:
pygame.display.quit()
# pygame.quit()
return
# press space to get screenshot then clear the screen
if event.key == pygame.K_SPACE:
self.count = self.count + 1
pygame.image.save(self.screen,"keyboard"+str(self.count)+".jpg")
self.screen.fill((255, 255, 255))
if event.key == pygame.K_w:
# Go Up
app.keyboardControl("Up")
if event.key == pygame.K_s:
# Go Down
app.keyboardControl("Down")
if event.key == pygame.K_a:
# Go Left
app.keyboardControl("Left")
if event.key == pygame.K_d:
# Go Right
app.keyboardControl("Right")
if event.key == pygame.K_UP:
# Pen up
app.keyboardControl("PenUp")
if event.key == pygame.K_DOWN:
# Pen down
app.keyboardControl("PenDown")
elif event.type == pygame.MOUSEBUTTONDOWN:
#print "Down"
self.brush.start_draw(event.pos)
elif event.type == pygame.MOUSEMOTION:
if self.brush.drawing:
#print "Draw"
self.brush.draw(event.pos)
elif event.type == pygame.MOUSEBUTTONUP:
#print "Up"
self.brush.end_draw()
pygame.draw.rect(self.screen, (255, 255, 255), (0, 0, 200, 120))
pygame.display.update()
def __init__(self, reward_list):
self.motionProxy = ALProxy("ALMotion")
self.reward_list = reward_list
self.draw = drawAction.drawAction()
self.reward_count = 0
self.action_count = 0
"""
Arm movement parameter
"""
self.names = ["RElbowRoll", "RShoulderRoll"]
self.isAbsolute = True
self.timeLists = [2, 2]
def update(sim, QLearning, num_episode, model):
app = drawAction.drawAction()
for episode in range(num_episode):
print "\nEpisode: ", episode + 1
# Initial sate
state = sim.reset()
while True:
# refresh simultor
sim.refresh()
# Choose action based on the state
action = QLearning.choose_action(str(state))
# print "action: ", action
# take action and get next state and reward
# Change string action to tuple
if len(action) > 5:
# Tuple
format_action = literal_eval(action)
else:
# String
format_action = action
next_state, reward, done = sim.step(state, format_action)
# Renew the q_table
QLearning.renew_qtable(str(state), action, reward, str(next_state))
state = next_state
if done:
break
if model == "Robot":
app.tts.say(
"Episode finish, please press Enter to clean the painter.")
print "Episode finish, please press Enter to clean the painter."
time.sleep(2) # Have 2 seconds to press Enter Key
# Save the q_table
QLearning.q_table.to_pickle("q_table.pkl")
print "Simulation finish."
if model == "Simulator":
print "Close the window"
sim.destroy()
def humanWrite(self):
self.count = 0
app = drawAction.drawAction()
self.screen.fill((255, 255, 255))
self.printText("Press 'Enter' when finished.", "Times New Roman", 30, 10, 10, (255, 0, 0))
self.printText("Press 'Space' to rewrite.", "Times New Roman", 30, 10, 50, (255, 0, 0))
while True:
# max fps limit
self.clock.tick(30)
for event in pygame.event.get():
if event.type == pygame.QUIT:
return
elif event.type == pygame.KEYDOWN:
# press esc to clear screen
if event.key == pygame.K_ESCAPE:
return
# press space to get screenshot then clear the screen
if event.key == pygame.K_RETURN:
pygame.image.save(self.screen, "humanWrite" + str(self.count) + ".jpg")
# Save the central point to file:
with open('imageList.txt', 'w') as file: # overwrite the file
for draw in self.brush.drawList:
file.write(str(draw)+'\n')
file.close()
app.tts.say("Save data")
pygame.display.quit()
return
# pygame.quit()
if event.key == pygame.K_SPACE:
self.screen.fill((255, 255, 255))
self.printText("Press 'Enter' when finished.", "Times New Roman", 30, 10, 10, (255, 0, 0))
self.printText("Press 'Space' to rewrite.", "Times New Roman", 30, 10, 50, (255, 0, 0))
self.brush.drawList = []
elif event.type == pygame.MOUSEBUTTONDOWN:
self.brush.start_draw(event.pos)
elif event.type == pygame.MOUSEMOTION:
if self.brush.drawing:
self.brush.draw(event.pos)
elif event.type == pygame.MOUSEBUTTONUP:
self.brush.end_draw()
pygame.display.update()
Set the Broker
"""
parser = argparse.ArgumentParser()
parser.add_argument("-ip", type=str, default="localhost",
help="Robot ip address")
parser.add_argument("-port", type=int, default=9559,
help="Robot port number")
args = parser.parse_args()
myBroker = ALBroker("myBroker",
"0.0.0.0", # Listen to anyone
0, # Find a free port and use it
args.ip, # Parent broker ip
args.port) # Parent broker port
app = drawAction.drawAction()
# Stop awareness
app.awareness.stopAwareness()
# Go to initial state
app.tts.say("Go to initial state")
print "Go to initial state"
app.goToInitialPos()
first_time = True
# Mode choose
global touchCommand
touchCommand = ReactToTouchModule("touchCommand")
touchCommand.touch_list = []
touchCommand.subscribe("FrontTactileTouch", "MiddleTactileTouch", "RearTactileTouch")