def game_round(self,position): # set round speed and guess speed = 1./(1+position) guess = -1 stop = False # turn on GPIO pins for i in pinList[position:]: GPIO.setup(i, GPIO.OUT) GPIO.output(i, GPIO.HIGH) # turn on correct guesses for i in pinList[0:position]: GPIO.output(i, GPIO.LOW) #start kbhit kb = KBHit() # start loop while True: for i in pinList[position:]: GPIO.output(i, GPIO.LOW) # check keypress if kb.kbhit(): c = kb.getch() if ord(c) != 27: stop = True guess = pinList.index(i)-1 break time.sleep(speed) # check keypress if stop: break pinList.reverse() for i in pinList[0:len(pinList)-position]: GPIO.output(i,GPIO.HIGH) if kb.kbhit(): c = kb.getch() if ord(c) != 27: stop = True guess = len(pinList)-(pinList.index(i)+1) pinList.reverse() time.sleep(speed) if stop: break pinList.reverse() # check results print guess print position return (guess == position)
def main(self):
kb = KBHit() # needed for windows to handle keyboard interrupt
print('Hit ESC to exit')
try:
#time.sleep(1)
#self.request_port_mapping()
time.sleep(0.5)
self.set_pin_mode(13, AsipClient.OUTPUT)
time.sleep(0.5)
self.set_pin_mode(2, AsipClient.INPUT_PULLUP)
time.sleep(0.5)
except Exception as e:
sys.stdout.write("Exception: caught {} in setting pin mode".format(e))
while True:
if kb.kbhit():
c = kb.getch()
if ord(c) == 27: # ESC
kb.set_normal_term()
break
try:
self.digital_write(13, 1)
time.sleep(1.25)
self.digital_write(13, 0)
time.sleep(1.25)
except Exception as e:
sys.stdout.write("Exception: caught {} in digital_write".format(e))
def main(self):
if os.name == 'nt':
kb = KBHit() # needed for windows to handle keyboard interrupt
sys.stdout.write('Hit ESC to exit\n')
try:
time.sleep(0.5)
self.asip.set_pin_mode(13, self.asip.OUTPUT)
time.sleep(0.5)
except Exception as e:
sys.stdout.write("Exception caught while setting pin mode: {}\n".format(e))
self.thread_killer()
sys.exit(1)
while True:
if os.name == 'nt':
if kb.kbhit():
c = kb.getch()
if ord(c) == 27: # ESC
kb.set_normal_term()
break
try:
self.asip.digital_write(13, self.asip.HIGH)
time.sleep(1.25)
self.asip.digital_write(13, self.asip.LOW)
time.sleep(1.25)
except (KeyboardInterrupt, Exception) as e:
sys.stdout.write("Caught exception in main loop: {}\n".format(e))
self.thread_killer()
sys.exit()
def talker():
lista_threads= []
pub = rospy.Publisher('ipcam', String)
rospy.init_node('ipcam', anonymous=True)
#rate = rospy.Rate(10) # 10hz
kbhit = KBHit()
print "Press i to move up the camera\n" \
"Press k to move down the camera\n" \
"Press j to move right the camera\n" \
"Press l to move left the camera\n" \
"Press q to home position the camera\n" \
"Press s to get another snapshot\n" \
"Press v to get video\n\n"
try:
while not rospy.is_shutdown():
if kbhit.kbhit():
print "Press i to move up the camera\n" \
"Press k to move down the camera\n" \
"Press j to move right the camera\n" \
"Press l to move left the camera\n" \
"Press q to home position the camera\n" \
"Press s to get another snapshot\n" \
"Press v to get video\n\n"
char = kbhit.getch()
if char == 's':
thread=threading.Thread(target=getImage, args=(False,))
thread.start()
time.sleep(1)
lista_threads.append(thread)
elif char == 'v':
thread=threading.Thread(target=getImage, args=(True,))
thread.start()
time.sleep(1)
lista_threads.append(thread)
else:
rospy.loginfo(char)
pub.publish(char)
char = ""
time.sleep(1)
except KeyboardInterrupt:
for each in lista_threads:
while each.isAlive():
pass
developer_mode = False
fruits = []
snake = create_snake(width, height)
create_apple(width, height, fruits, snake)
direction = (1, 0)
print(field_to_text(width, height, snake, fruits) + '\n')
p = time.time()
kb = KBHit()
while True:
c = time.time()
time_delta = 0.001 if developer_mode else 0.1
if c - p < time_delta:
time.sleep(time_delta - (c - p))
p = time.time()
if kb.kbhit():
c = kb.getch()
if ord(c) == 27: # ESC
break
elif ord(c) in directions_codes:
direction = directions_codes[ord(c)]
elif developer_mode:
direction = (0, 0)
if not update_screen(width, height, snake, fruits, direction):
break
elif not developer_mode:
if not update_screen(width, height, snake, fruits, direction):
break
def run(self):
kb = KBHit()
selectedData = []
dataToAverage = []
print('Hit ESC to exit')
while True:
if kb.kbhit():
c = kb.getch()
if ord(c) == 27: # ESC
kb.set_normal_term()
print "Quitting!"
os._exit(0)
now = time.time()
due = now - self.parent.offset
search_from = self.parent.index - 1
if search_from == -1:
search_from = self.parent.buffer_size - 1
timestamp = 0
nextTimestamp = 0
self.waitForNewData = 0.5
self.parent.mutex.acquire()
lastjvalue = 0
dataArray = []
for theid in range(self.parent.channel_count):
data = self.parent.outbuffers[int(theid)][self.parent.index]
for i in range(search_from,
search_from - self.parent.buffer_size, -1):
j = i if i >= 0 else i + self.parent.buffer_size
timestamp = self.parent.timestamp_buffer[j]
if timestamp is not None and timestamp <= due:
data = self.parent.outbuffers[int(theid)][j]
lastjvalue = j
if (j < self.parent.buffer_size - 2):
nextTimestamp = self.parent.timestamp_buffer[j + 1]
else:
nextTimestamp = self.parent.timestamp_buffer[0]
break
if data is not None:
dataArray.append(data)
#print "data array: ", dataArray
for f in config.streams:
stream = f['stream']
if len(dataArray) > stream:
#print "stream = ", stream, dataArray[stream]
if 'scale' in f:
dataArray[stream] = self.format.scale(
dataArray[stream], f['scale'])
if 'type' in f and f['type'] == int:
#msgString += str(int(dataArray[stream]))
selectedData.append(int(dataArray[stream]))
else:
#msgString += str(dataArray[stream])
selectedData.append(dataArray[stream])
if len(selectedData) == len(config.streams):
self.sendMessage(selectedData)
selectedData = []
if timestamp is not None and nextTimestamp is not None:
waitTime = nextTimestamp - timestamp
if waitTime > 0:
time.sleep(waitTime)
else:
print "timing out waiting for new data - time < 0"
time.sleep(self.waitForNewData)
else:
print "timing out waiting for new data - time or nextTime is None"
time.sleep(self.waitForNewData)
self.parent.mutex.release()
for i in range(len(pawns[8])):
if pawns[8][i].get_shield_active() is True:
if (now - pawns[8][i].get_timestamp()).seconds > 5:
pawns[8][i].deactivate_shield()
if pawns[8][i].get_dragon_active() is True:
if (now - pawns[8][i].get_dragon_timestamp()).seconds > 5:
pawns[8][i].deactivate_dragon()
if pawns[8][i].get_dragon_active() is True:
pawns[8][i].set_dragon_sprite(offsets[idx])
speed_boost += 0.005
speed_boost_times.append(now)
pawns[8][i].set_position(np.array([GROUND_SIZE[0] - 12, 0]))
if _KBHIT.kbhit():
inp = _KBHIT.getch().lower()
if inp == 'q':
break
elif inp == 'e' and pawns[8][0].get_dragon_active() is False:
pawns[9].append(
Bullet([
pawns[8][0].get_position()[0],
pawns[8][0].get_position()[1] + 2
], ObjNumber, 0))
ObjNumber += 1
else:
pawns[8][0].control(inp, offsets[i], GROUND_SIZE[0])
for i in range(len(pawns[5])):
pawns[8][0] = pawns[5][i].on_trigger(pawns[8][0])
batch_size=50,
nb_epoch=20,
verbose=1,
validation_data=(X_val, Y_v),
callbacks=[early_stopping])
# most recent loss hist.history["loss"][-1]
r, rmse, _ = learnLib.assess_2dmodel(model, X_val, Y_v)
models[args] = r, rmse
print("Model r: ", r)
print("Model rmse: ", rmse)
if rmse < prevLoss:
prevLoss = rmse
maxModel = model
maxModelOutShape = outshape
while kb.kbhit():
try:
if "q" in kb.getch():
print("quiting due to user pressing q")
stop = True
except UnicodeDecodeError:
pass
if stop:
break
X_test, Y_t = ns.getTestData()
X_test = np.reshape(X_test, (-1, 1, 1, 120))
Y_test = np.array(list(map(lambda x: interpolateYs(x, maxModelOutShape), Y_t)))
def main():
random.seed(time.time())
# Initialise colorama
init()
# Clear screen
print(chr(27) + '[2j')
print('\033c')
print('\x1bc')
# Get terminal size
(ncols, nlines) = shutil.get_terminal_size()
# Initialise screen
screen = Window(0, 1, ncols, nlines - 1, f"{config.bkgd} ")
# Initialise keyboard
kb = KBHit()
# Create and draw the paddle
paddle = Paddle(((ncols - 1) - config.paddle["dim"][0]) // 2, nlines - 4,
screen)
last_update = 0
start = time.time()
score = 0
lives = 3
curr_level = 1
# List to store shooters
shooters = [None, None]
while curr_level <= 3:
# Create and draw bricks
boss = None
if curr_level == 1:
bricks, powerups = levels.level_one(screen)
elif curr_level == 2:
bricks, powerups = levels.level_two(screen)
elif curr_level == 3:
bricks, powerups = levels.level_three(screen)
boss = Boss(ncols // 2, 5, screen)
else:
sys.exit()
if lives == 0:
break
while lives:
# Reset paddle location
paddle.x = ((ncols - 1) - config.paddle["dim"][0]) // 2
paddle.powerup = None
# Create and draw the ball
balls = [
Ball(random.randrange(paddle.x, paddle.x + paddle.width),
nlines - 5, list(config.ball["speed"]), 1, screen)
]
# List to store bullets
bullets = []
shoot_paddle = [0]
# List to store bombs
bombs = []
tick = 0
while len(balls):
if (time.time() - last_update > config.tick_interval):
tick += 1
for i in range(ncols):
print(f"{Cursor.POS(1+i, 1)}{Back.BLACK} ", end='')
statusline = f"{Cursor.POS(1, 1)}Level: {curr_level} "
statusline += f"Score: {score} "
statusline += f"Time: {int(time.time() - start)} "
statusline += f"Lives: {lives} "
statusline += f"Shoot Paddle: {int(shoot_paddle[0] * config.tick_interval)} "
if boss:
statusline += "Boss Health: ["
for i in range(0, boss.health, 10):
statusline += "•"
for i in range(0, 100 - boss.health, 10):
statusline += " "
statusline += "]"
print(statusline, end='')
last_update = time.time()
change_level = False
direction = 0
if kb.kbhit():
c = kb.getch()
if ord(c) == 27:
sys.exit(0)
if c == 'a':
direction = -1
elif c == 'd':
direction = 1
elif c == ' ':
# Activate balls
for ball in balls:
ball.paused = False
elif c == '1' or c == '2' or c == '3':
curr_level = int(c) if curr_level != 3 else 4
change_level = True
break
# Create new bullets if needed
shoot_paddle[0] = max(0, shoot_paddle[0] - 1)
if shoot_paddle[0] and shoot_paddle[0] % config.bullet[
"rate"] == 0:
bullets.append(
Bullet(max(paddle.x, 0), paddle.y,
config.bullet["speed"], screen))
bullets.append(
Bullet(
min(paddle.x + paddle.width,
screen.get_screen_size()[1] - 1), paddle.y,
config.bullet["speed"], screen))
# Create bomb if needed
if boss and tick % config.bomb["rate"] == 0:
bombs.append(
Bomb(boss.x + 5, boss.y + 3, config.bomb["speed"],
screen))
# Create shooters if needed
if shoot_paddle[0] and not shooters[0]:
shooters[0] = Object(paddle.x, paddle.y - 1, 1, 1,
config.paddle["color"], screen)
if shoot_paddle[0] and not shooters[1]:
shooters[1] = Object(paddle.x + paddle.width - 1,
paddle.y - 1, 1, 1,
config.paddle["color"], screen)
# Create brick layer on boss level if health is 50
if boss and boss.health == 80 and not boss.done[0]:
bricks.append([])
brick_count = screen.get_screen_size()[1] // \
(config.brick["dim"][0] + 5)
for i in range(brick_count):
bricks[1].append(
Brick(i * (config.brick["dim"][0] + 5), 9, 1,
screen))
boss.done[0] = True
# Create brick layer on boss level if health is 20
if boss and boss.health == 20 and not boss.done[1]:
brick_count = screen.get_screen_size()[1] // \
(config.brick["dim"][0] + 5)
bricks.append([])
for i in range(brick_count):
bricks[2].append(
Brick(i * (config.brick["dim"][0] + 5), 13, 1,
screen))
boss.done[1] = True
# Clear screen
screen.clear()
# Move the paddle
paddle.move(direction, balls)
# Move the powerups
to_delete = []
for powerup in powerups:
object = None
if powerup.type == "paddle":
object = paddle
elif powerup.type == "ball":
object = balls
else:
object = shoot_paddle
if not powerup.move(paddle, object, tick):
to_delete.append(powerup)
powerups = [
powerup for powerup in powerups
if powerup not in to_delete
]
# Move the ball
to_delete = []
for ball in balls:
delete, d_score = ball.move(bricks, paddle, boss)
if not delete:
to_delete.append(ball)
score += d_score
balls = [ball for ball in balls if ball not in to_delete]
# Delete shooters if powerup is over
if shoot_paddle[0] == 0:
shooters = [None, None]
# Move the bullets
to_delete = []
for bullet in bullets:
delete, d_score = bullet.move(bricks)
if not delete:
to_delete.append(bullet)
score += d_score
bullets = [
bullet for bullet in bullets if bullet not in to_delete
]
# Move the bombs
to_delete = []
lose_life = False
for bomb in bombs:
delete, lose_life = bomb.move(paddle)
if not delete:
to_delete.append(bomb)
if lose_life:
break
if lose_life:
break
bombs = [bomb for bomb in bombs if bomb not in to_delete]
# Move the boss
if boss:
boss.move(paddle.x + paddle.width // 2)
# Move the shooter
if shooters[0]:
shooters[0].x = paddle.x
if shooters[1]:
shooters[1].x = paddle.x + paddle.width - 1
# Update bricks
for layer in bricks:
for brick in layer:
brick.rainbow(tick)
# Don't move bricks on boss level
if curr_level < 3:
brick.move(tick)
if brick.y + brick.height > paddle.y:
sys.exit()
brick.update()
# Update paddle
paddle.update()
# Update shooters
for shooter in shooters:
if shooter:
shooter.update()
# Update powerups
for powerup in powerups:
powerup.update()
# Update ball
for ball in balls:
ball.update()
# Update bullets
for bullet in bullets:
bullet.update()
# Update bombs
for bomb in bombs:
bomb.update()
# Boss update
if boss:
boss.update()
screen.draw()
# Check if all breackable bricks are broken on non boss levels
if curr_level < 3:
change_level = True
for layer in bricks:
for brick in layer:
if brick._strength != 4:
change_level = False
break
if not change_level:
break
else:
if boss.health <= 0:
change_level = True
if change_level:
curr_level += 1
break
if change_level:
break
lives -= 1
def run(self):
kb = KBHit()
selectedData = []
dataToAverage = []
print('Hit ESC to exit')
while True:
if kb.kbhit():
c = kb.getch()
if ord(c) == 27: # ESC
kb.set_normal_term()
print "Quitting!"
os._exit(0)
now = time.time()
due = now - self.parent.offset
search_from = self.parent.index - 1
if search_from == - 1:
search_from = self.parent.buffer_size - 1
timestamp = 0
nextTimestamp = 0
self.waitForNewData = 0.5
self.parent.mutex.acquire()
lastjvalue = 0
dataArray = []
for theid in range(self.parent.channel_count):
data = self.parent.outbuffers[int(theid)][self.parent.index]
for i in range(search_from, search_from - self.parent.buffer_size, -1):
j = i if i >= 0 else i + self.parent.buffer_size
timestamp = self.parent.timestamp_buffer[j]
if timestamp is not None and timestamp <= due:
data = self.parent.outbuffers[int(theid)][j]
lastjvalue = j
if(j<self.parent.buffer_size-2):
nextTimestamp = self.parent.timestamp_buffer[j+1]
else:
nextTimestamp = self.parent.timestamp_buffer[0]
break
if data is not None:
dataArray.append(data)
#print "data array: ", dataArray
for f in config.streams:
stream = f['stream']
if len(dataArray) > stream:
#print "stream = ", stream, dataArray[stream]
if 'scale' in f:
dataArray[stream] = self.format.scale(dataArray[stream], f['scale'])
if 'type' in f and f['type'] == int:
#msgString += str(int(dataArray[stream]))
selectedData.append(int(dataArray[stream]))
else:
#msgString += str(dataArray[stream])
selectedData.append(dataArray[stream])
if len(selectedData) == len(config.streams) :
self.sendMessage(selectedData)
selectedData = []
if timestamp is not None and nextTimestamp is not None:
waitTime = nextTimestamp - timestamp
if waitTime > 0:
time.sleep(waitTime)
else:
print "timing out waiting for new data - time < 0"
time.sleep(self.waitForNewData)
else:
print "timing out waiting for new data - time or nextTime is None"
time.sleep(self.waitForNewData)
self.parent.mutex.release()
class GraspExecuter(object):
def __init__(self):
self._moveit_wrapper = MoveitWrapper()
self._moveit_wrapper.init_moveit_commander()
rospy.init_node('grasp_player')
self._moveit_wrapper.setup()
self._kin = boris_kinematics(root_link="left_arm_base_link")
self._boris = BorisRobot(moveit_wrapper=self._moveit_wrapper)
self._scene = self._moveit_wrapper.scene()
self._planning_frame = self._moveit_wrapper.robot().get_planning_frame(
)
self._tf_buffer = tf2_ros.Buffer()
self._listener = tf2_ros.TransformListener(self._tf_buffer)
self._br = tf.TransformBroadcaster()
self._scene.remove_world_object("table")
self.add_table()
self._solution = None
self._grasp_waypoints = []
self._pre_grasp_plan = None
self._pre_grasp_plan2 = None
self._grasp_arm_joint_path = None
self._grasp_hand_joint_path = None
self._post_grasp_plan = None
self._grasp_service_client = GraspServiceClient()
self._boris.set_control_mode(mode="joint_impedance",
limb_name="left_arm")
self._arm_traj_generator = MinJerkTrajHelper()
self._hand_traj_generator = MinJerkTrajHelper()
self._pre_grasp_traj_generator = MinJerkTrajHelper()
self._post_grasp_traj_generator = MinJerkTrajHelper()
self._scan_waypoints = np.load(
"/home/earruda/Projects/boris_ws/src/boris-robot/boris_tools/scripts/scan_waypoints2.npy"
)
self._kbhit = KBHit()
def scan_object(self):
command = "rosservice call /grasp_service/acquire_cloud"
self._boris.set_vel_accel_scaling("left_arm", 0.45, 0.45)
for waypoint in self._scan_waypoints:
# goto
self._boris.goto_with_moveit("left_arm", waypoint)
# scan
process = subprocess.Popen(command.split(), stdout=subprocess.PIPE)
output, error = process.communicate()
print output
if rospy.is_shutdown():
break
self._boris.set_vel_accel_scaling("left_arm", 0.25, 0.25)
def remove_collision_object(self, name):
self._scene.remove_world_object(name)
rospy.sleep(0.5)
def add_table(self):
self.remove_collision_object("table")
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self._planning_frame
p.pose.position.x = 0.55
p.pose.position.y = 0
p.pose.position.z = -0.37 #-0.34
p.pose.orientation.w = 1.0
self._scene.add_box("table", p, (0.87, 1.77, 0.04))
rospy.sleep(2)
def add_object_guard(self, grasp_solution):
self.remove_collision_object("guard")
# print "Got solution: ", grasp_solution
cloud_centroid = grasp_solution['cloud_centroid']
min_pt = grasp_solution['cloud_min_pt']
max_pt = grasp_solution['cloud_max_pt']
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self._planning_frame
p.pose.position.x = cloud_centroid[0]
p.pose.position.y = cloud_centroid[1]
p.pose.position.z = cloud_centroid[2]
p.pose.orientation.w = 1.0
self._scene.add_box("guard", p,
(max_pt[0] - min_pt[0], max_pt[1] - min_pt[1],
max_pt[2] - min_pt[2]))
rospy.sleep(0.1)
def get_solution(self):
self._solution = self._grasp_service_client.get_grasp_solution()
self.add_table()
self.add_object_guard(self._solution)
self._grasp_waypoints = solution2carthesian_path(
self._solution, self._tf_buffer)
group = self._moveit_wrapper.get_group("left_hand_arm")
group.set_goal_joint_tolerance(0.001) # default 0.0001
group.set_goal_position_tolerance(0.01) # default 0.0001
group.set_goal_orientation_tolerance(0.001) # default 0.001
#self._grasp_waypoints[0].position.z += 0.15
dx = self._grasp_waypoints[1].position.x - self._grasp_waypoints[
0].position.x
dy = self._grasp_waypoints[1].position.y - self._grasp_waypoints[
0].position.y
dz = self._grasp_waypoints[1].position.z - self._grasp_waypoints[
0].position.z
dir_vec = np.array([dx, dy, dz])
dist = np.linalg.norm(dir_vec)
dir_vec /= dist
dir_vec *= 0.0 #0.05
target = geometry_msgs.msg.Pose()
for n_tries in range(50):
rand_offset_x = np.random.randn() * np.sqrt(0.0001)
rand_offset_y = np.random.randn() * np.sqrt(0.0001)
rand_offset_z = np.maximum(
0.15 + np.random.randn() * np.sqrt(0.0001), 0.10)
target.position.x = self._grasp_waypoints[0].position.x - dir_vec[
0] + rand_offset_x
target.position.y = self._grasp_waypoints[0].position.y - dir_vec[
1] + rand_offset_y
print rand_offset_x, rand_offset_y, rand_offset_z
target.position.z = self._grasp_waypoints[
0].position.z + rand_offset_z
target.orientation = self._grasp_waypoints[0].orientation
self._pre_grasp_plan = self._boris.get_moveit_cartesian_plan(
"left_hand_arm", target)
is_executable = len(
self._pre_grasp_plan.joint_trajectory.points) > 0
if is_executable:
break
# if is_executablediag_add:
# # current_angles = self._boris.angles("left_arm")
# # joint_positions = self._pre_grasp_plan.joint_trajectory.points[-1].positions
# # self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), joint_positions)
# # rospy.sleep(1.0)
# self._grasp_arm_joint_path, fraction = self._boris.compute_cartesian_path_moveit("left_hand_arm",self._grasp_waypoints[:3])
# # Set back to current
# # self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), current_angles)
# is_executable = fraction >= 0.85
if is_executable:
self._pre_grasp_pose = msg2Transform3(self._grasp_waypoints[0])
print "Hand joints %d ", len(
self._pre_grasp_plan.joint_trajectory.points)
self._pre_grasp_traj_generator.set_waypoints(
self._pre_grasp_plan.joint_trajectory)
rospy.loginfo(
"Pre grasp plan length %d" %
(len(self._pre_grasp_plan.joint_trajectory.points), ))
rospy.loginfo("Pre grasp plan total time %f" %
(self._pre_grasp_plan.joint_trajectory.points[-1].
time_from_start.to_sec(), ))
# state = self._moveit_wrapper.robot().get_current_state()
# print "Robot state"
# print state
#group = self._moveit_wrapper.get_group("left_hand_arm")
return is_executable
def update_step(self, step, time_steps, incr):
step = step + incr
if step >= len(time_steps):
step = len(time_steps) - 1
elif step < 0:
step = 0
return step
def goto_pregrasp(self):
self._boris.execute_moveit_plan("left_hand_arm", self._pre_grasp_plan)
def plan_grasp(self):
self.remove_collision_object("guard")
self.remove_collision_object("table")
# self._pre_grasp_plan2, fraction2 = self._boris.compute_cartesian_path_moveit("left_hand_arm",[self._grasp_waypoints[:1]])
self._grasp_arm_joint_path, fraction = self._boris.compute_cartesian_path_moveit(
"left_hand_arm",
self._grasp_waypoints[:3],
eef_step=0.01,
jump_threshold=50.0)
# Set back to current
# self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), current_angles)
is_executable = fraction >= 0.95
## try to plan final goal in case cartesian path
if not is_executable:
rospy.logwarn(
"Failed to plan cartesian path passing through all waypoints. Trying last one only."
)
self._grasp_arm_joint_path = self._boris.get_moveit_cartesian_plan(
"left_hand_arm", self._grasp_waypoints[2])
is_executable = len(
self._grasp_arm_joint_path.joint_trajectory.points) > 0
if not is_executable:
rospy.logwarn(
"Grasp not executable, maybe try again or try new grasp")
else:
self._grasp_arm_joint_path = self._grasp_arm_joint_path.joint_trajectory
self._arm_traj_generator.set_waypoints(self._grasp_arm_joint_path)
rospy.loginfo("Grasp path length %d" %
(len(self._grasp_arm_joint_path.points), ))
rospy.loginfo("Grasp path total time %f" %
(self._grasp_arm_joint_path.points[-1].
time_from_start.to_sec(), ))
total_time = self._grasp_arm_joint_path.points[
-1].time_from_start.to_sec()
self._grasp_hand_joint_path = solution2hand_joint_path(
self._solution, self._boris, total_time)
print "Trajectory hand ", len(
self._solution['joint_trajectory'][0])
self._hand_traj_generator.set_waypoints(
self._grasp_hand_joint_path)
rospy.loginfo("Hand path length %d" %
(len(self._grasp_hand_joint_path.points), ))
rospy.loginfo("Hand path total time %f" %
(self._grasp_hand_joint_path.points[-1].
time_from_start.to_sec(), ))
return is_executable
def plan_post_grasp(self):
self.remove_collision_object("guard")
self.remove_collision_object("table")
self._grasp_waypoints[3].position.z += 0.08
self._post_grasp_plan = self._boris.get_moveit_cartesian_plan(
"left_hand_arm", self._grasp_waypoints[3])
# Set back to current
# self._moveit_wrapper.set_start_state("left_hand_arm", self._boris.joint_names("left_arm"), current_angles)
is_executable = len(self._post_grasp_plan.joint_trajectory.points) > 0
if not is_executable:
rospy.logwarn(
"Grasp not executable, maybe try again or try new grasp")
else:
self._post_grasp_plan = self._post_grasp_plan.joint_trajectory
self._post_grasp_traj_generator.set_waypoints(
self._post_grasp_plan)
rospy.loginfo("Grasp path length %d" %
(len(self._post_grasp_plan.points), ))
rospy.loginfo(
"Grasp path total time %f" %
(self._post_grasp_plan.points[-1].time_from_start.to_sec(), ))
return is_executable
def execute_pre_grasp(self):
rospy.loginfo("Pre Grasp Execution!!")
key = None
time_steps = np.linspace(0.0, 1.0, 600)
step = 0
def exec_step(step):
joint_goal = self._pre_grasp_traj_generator.get_point_t(
time_steps[step])
print "CurrArmState:", self._boris.angles('left_arm')
print "ArmGoal[%.2f]: " % (
time_steps[step], ), joint_goal.time_from_start.to_sec(
), " step=", step, " pos: ", joint_goal.positions
cmd = self._boris.cmd_joint_angles(angles=joint_goal.positions,
execute=True)
# self._boris.goto_joint_angles('left_hand',hand_goal.positions)
loop_rate = rospy.Rate(30.0)
play_forward = False
play_backward = False
while not rospy.is_shutdown() and key != 'q':
if self._kbhit.kbhit():
key = self._kbhit.getch()
if key == '.':
rospy.loginfo("Step %d" % (step, ))
step = self.update_step(step, time_steps, 1)
exec_step(step)
if key == ',':
step = self.update_step(step, time_steps, -1)
exec_step(step)
rospy.loginfo("Step %d" % (step, ))
if key == ']':
play_forward = True
play_backward = False
rospy.loginfo("Playing forward")
elif key == '[':
play_forward = False
play_backward = True
rospy.loginfo("Playing backward")
elif key == 'p' and (play_forward or play_backward):
play_forward = False
play_backward = False
rospy.loginfo("Halt open loop playing")
if play_forward:
step = self.update_step(step, time_steps, 1)
exec_step(step)
rospy.loginfo("Step %d" % (step, ))
elif play_backward:
step = self.update_step(step, time_steps, -1)
exec_step(step)
rospy.loginfo("Step %d" % (step, ))
loop_rate.sleep()
rospy.loginfo("Leaving Pre Grasp Execution!!")
def execute_grasp(self):
rospy.loginfo("Grasp Execution!!")
key = None
rospy.loginfo("Grasp path length %d" %
(len(self._grasp_arm_joint_path.points), ))
rospy.loginfo(
"Grasp path total time %f" %
(self._grasp_arm_joint_path.points[-1].time_from_start.to_sec(), ))
time_steps = np.linspace(0.0, 1.0, 300)
time_steps_arm = np.linspace(0.0, 1.0, 100)
step = 0
step_arm = 0
def step_grasp(step, arm_step):
joint_goal = self._arm_traj_generator.get_point_t(
time_steps_arm[arm_step])
hand_goal = self._hand_traj_generator.get_point_t(time_steps[step])
print "HandGoal: ", hand_goal.time_from_start.to_sec(
), " step=", step, " pos: ", hand_goal.positions
print "CurrArmState:", self._boris.angles('left_arm')
print "ArmGoal: ", hand_goal.time_from_start.to_sec(
), " step=", step, " pos: ", joint_goal.positions
cmd = self._boris.cmd_joint_angles(angles=joint_goal.positions,
execute=True)
self._boris.goto_joint_angles('left_hand', hand_goal.positions,
hand_goal.time_from_start.to_sec())
loop_rate = rospy.Rate(30.0)
play_forward = False
play_backward = False
while not rospy.is_shutdown() and key != 'q':
key = None
if self._kbhit.kbhit():
key = self._kbhit.getch()
if key == '.':
step = self.update_step(step, time_steps, 1)
step_arm = self.update_step(step_arm, time_steps_arm, 1)
rospy.loginfo("Step %d" % (step, ))
step_grasp(step, step_arm)
if key == ',':
step = self.update_step(step, time_steps, -1)
step_arm = self.update_step(step_arm, time_steps_arm, -1)
rospy.loginfo("Step %d" % (step, ))
step_grasp(step, step_arm)
wrench = self._boris.wrench()
force = np.array([
wrench.wrench.force.x, wrench.wrench.force.y,
wrench.wrench.force.z
])
# If force is larger than 5 we stop
force_norm = np.linalg.norm(force)
if force_norm >= 15.0:
key = '['
rospy.logwarn("External forces too high %.3f" % (force_norm, ))
# else:
# rospy.loginfo("External forces %.3f"%(force_norm,))
if key == ']':
play_forward = True
play_backward = False
rospy.loginfo("Playing forward")
elif key == '[':
play_forward = False
play_backward = True
rospy.loginfo("Playing backward")
elif key == 'p' and (play_forward or play_backward):
play_forward = False
play_backward = False
rospy.loginfo("Halt open loop playing")
elif key == 'c':
self._boris.goto_joint_angles('left_hand', [1.0], 0.01)
elif key == 'o':
self._boris.goto_joint_angles('left_hand', [0.0], 0.01)
if play_forward:
step = self.update_step(step, time_steps, 1)
step_arm = self.update_step(step_arm, time_steps_arm, 1)
step_grasp(step, step_arm)
rospy.loginfo("Step %d" % (step, ))
elif play_backward:
step = self.update_step(step, time_steps, -1)
step_arm = self.update_step(step_arm, time_steps_arm, -1)
step_grasp(step, step_arm)
rospy.loginfo("Step %d" % (step, ))
loop_rate.sleep()
rospy.loginfo("Leaving Grasp Execution!!")
def step_execution(self, step, time_steps, trajectory_generator,
command_func):
joint_goal = trajectory_generator.get_point_t(time_steps[step])
command_func(joint_goal.positions)
def execute_post_grasp(self):
rospy.loginfo("Post Grasp Execution!!")
key = None
time_steps = np.linspace(0.0, 1.0, 200)
#time_steps_arm = np.linspace(0.0,1.0,100)
step = 0
loop_rate = rospy.Rate(30.0)
play_forward = False
play_backward = False
while not rospy.is_shutdown() and key != 'q':
key = None
if self._kbhit.kbhit():
key = self._kbhit.getch()
if key == '.':
step = self.update_step(step, time_steps, 1)
rospy.loginfo("Step %d" % (step, ))
self.step_execution(step, time_steps,
self._post_grasp_traj_generator,
self._boris.cmd_joint_angles)
if key == ',':
step = self.update_step(step, time_steps, -1)
rospy.loginfo("Step %d" % (step, ))
self.step_execution(step, time_steps,
self._post_grasp_traj_generator,
self._boris.cmd_joint_angles)
if key == ']':
play_forward = True
play_backward = False
rospy.loginfo("Playing forward")
elif key == '[':
play_forward = False
play_backward = True
rospy.loginfo("Playing backward")
elif key == 'p' and (play_forward or play_backward):
play_forward = False
play_backward = False
rospy.loginfo("Halt open loop playing")
if play_forward:
step = self.update_step(step, time_steps, 1)
self.step_execution(step, time_steps,
self._post_grasp_traj_generator,
self._boris.cmd_joint_angles)
rospy.loginfo("Step %d" % (step, ))
elif play_backward:
step = self.update_step(step, time_steps, -1)
self.step_execution(step, time_steps,
self._post_grasp_traj_generator,
self._boris.cmd_joint_angles)
rospy.loginfo("Step %d" % (step, ))
loop_rate.sleep()
rospy.loginfo("Leaving Post Grasp Execution!!")
def run(self):
rospy.loginfo("Running!!")
loop_rate = rospy.Rate(30)
has_solution = False
has_plan = False
has_post_grasp_plan = False
while not rospy.is_shutdown():
key = self._kbhit.getch()
if key == "0":
has_solution = self.get_solution()
rospy.loginfo("Queried solution %s" % (has_solution, ))
elif key == "1" and has_solution:
#self.goto_pregrasp()
self.execute_pre_grasp()
elif key == "2" and has_solution:
if has_plan:
self.execute_grasp()
else:
rospy.logwarn(
"No grasp plan has been generated. Press 9 to attempt to generate one."
)
elif key == "3" and has_solution:
if has_post_grasp_plan:
self.execute_post_grasp()
else:
rospy.logwarn(
"No post-grasp plan has been generated. Press 8 to attempt to generate one."
)
elif key == "9":
has_plan = self.plan_grasp()
elif key == "8":
has_post_grasp_plan = self.plan_post_grasp()
elif key == "r":
self.remove_collision_object("table")
self.remove_collision_object("guard")
elif key == "t":
self.add_table()
elif key == "g" and has_solution:
self.add_object_guard(self._solution)
elif key == "e":
self.remove_collision_object("guard")
elif key == "s":
self.scan_object()
elif not has_solution:
rospy.logwarn("No grasp solution. Press 0.")
loop_rate.sleep()
class Hotkey:
def __init__(self, keys: str, mode: str = 'hotkey', window=None):
self.mode = mode
self.window = window
self.shot_job = None
self.battle_job = self.visit_room_job = self.cork_shop_job = self.monitor_job = Job(
)
self.kb = KBHit()
self.positions = [Position(_id) for _id in range(1, 10)]
self.keys = [
f'z+{i}'
for i in (list(range(1, 10)) if mode == 'hotkey' else []) +
list(keys)
]
self.add_hotkey()
def __user_command(self, key):
while self.kb.kbhit():
self.kb.getch()
if '1' <= key <= '9':
self.positions[int(key) - 1].record(self.monitor_job.is_alive())
elif self.monitor_job.is_alive():
pass
else:
self.__call(key)
def __call(self, key: str):
method_name = f'_Hotkey__cmd_{key}'
getattr(self, method_name)()
def __cmd_r(self):
if self.process_idle():
self.battle_job = Job(target=battle, args=(fake_window, ))
self.battle_job.start()
elif self.battle_job.is_pausing():
self.battle_job.resume()
def __cmd_v(self):
if self.process_idle():
self.visit_room_job = Job(target=visit_friend_room,
args=(fake_window, ))
self.visit_room_job.start()
elif self.visit_room_job.is_pausing():
self.visit_room_job.resume()
def __cmd_e(self):
if self.process_idle():
self.cork_shop_job = Job(target=cork_shop_exchange,
args=(fake_window, ))
self.cork_shop_job.start()
elif self.cork_shop_job.is_pausing():
self.cork_shop_job.resume()
def __cmd_i(self):
kirafan.screenshot()
def __cmd_s(self):
if self.mode == 'gui':
if self.window['_button_Start_'].GetText() == 'Stop':
self.window.write_event_value('_button_Start_', None)
elif self.window['_button_Visit Room_'].GetText() == 'Stop Visit':
self.window.write_event_value('_button_Visit Room_', None)
elif self.window['_button_Cork Shop_'].GetText(
) == 'Stop Exchange':
self.window.write_event_value('_button_Cork Shop_', None)
elif not self.process_idle():
logger.info('press stop now!, Please wait for a while')
self.safe_exit()
def __cmd_l(self):
uData.reload()
adb.reload()
kirafan.reload()
logger.info('kirafan-bot: reload configuration finish')
logger.info(f'kirafan-bot: region = {list(kirafan.region)}')
logger.info(f'kirafan-bot: adb use = {uData.setting["adb"]["use"]}')
logger.info(
f'kirafan-bot: quest setting = \x1b[41m{kirafan.quest_name}\x1b[0m'
)
def __cmd_k(self):
uData.adb_mode_switch()
adb.reload()
kirafan.adb_mode_switch()
logger.info(f'kirafan-bot: region = {list(kirafan.region)}')
logger.info(
f'kirafan-bot: adb use = \x1b[35m{uData.setting["adb"]["use"]}\x1b[0m'
)
tuple_files = kirafan.miss_icon_files()
if tuple_files:
print('miss icon files:')
for tuple_file in tuple_files:
print(f' {tuple_file[1]}')
print('you can press hotkey "z+c" to add a miss icon file')
def __cmd_m(self):
if not self.monitor_job.is_alive():
self.monitor_job = Job(target=monitor_mode)
self.monitor_job.start()
def __cmd_p(self):
for position in self.positions:
print(position, end='')
def __cmd_t(self):
print('')
adb.set_update_cv2_IM_cache_flag()
kirafan.objects_found_all_print()
kirafan.icons_found_all_print()
print('')
def __cmd_c(self):
if self.shot_job and self.shot_job.is_alive():
return
tuple_files = kirafan.miss_icon_files()
if tuple_files:
print('miss icon files:')
for i, tuple_file in enumerate(tuple_files):
print(f' {i}.{tuple_file[0]}')
sleep(0.1)
if self.kb.kbhit():
self.kb.getch()
number = input('select a number which you want to save icon: ')
if number.isnumeric() and 0 <= int(number) < len(tuple_files):
self.shot_job = Job(target=self.tutorial_screenshot,
args=[tuple_files[int(number)]])
self.shot_job.start()
else:
print('invaild input:', number)
else:
print('No miss icon file')
def __cmd_o(self):
kirafan.stop_once = not kirafan.stop_once
logger.info(
f'({str(kirafan.stop_once):>5}) kirafan-bot stop after current battle is completed'
)
def __cmd_x(self):
old = uData.setting['game_region'][:2]
new = game_region()
if old != new:
uData.save_location(*new)
self.__cmd_l()
def add_hotkey(self):
for key in self.keys:
keyboard.add_hotkey(key, self.__user_command, args=[key[-1]])
def remove_all_hotkey(self):
for key in self.keys:
keyboard.remove_hotkey(key)
def tutorial_screenshot(self, tuple_f: Tuple[str, str]):
"""
tuple_f = (fname, fpath)
"""
print(f'Missing {tuple_f[0]} picture')
print(
f'move mouse to the {tuple_f[0]} top left corner, then press hotkey "z+3"'
)
keyboard.wait('z+3')
print(
f'move mouse to the {tuple_f[0]} bottom right corner, then press hotkey "z+4"'
)
keyboard.wait('z+4')
Shot(tuple_f[0], tuple_f[1], self.positions[2].coord,
self.positions[3].coord).screenshot()
def wait(self, hotkey: str):
return keyboard.wait(hotkey)
def process_idle(self) -> bool:
return not (self.battle_job.is_alive()
or self.visit_room_job.is_alive()
or self.cork_shop_job.is_alive())
def safe_exit(self):
if self.battle_job.is_alive():
self.battle_job.stop()
self.battle_job.join()
if self.monitor_job.is_alive():
self.monitor_job.stop()
self.monitor_job.join()
if self.visit_room_job.is_alive():
self.visit_room_job.stop()
self.visit_room_job.join()
if self.cork_shop_job.is_alive():
self.cork_shop_job.stop()
self.cork_shop_job.join()
sleep(0.1)
while self.kb.kbhit():
self.kb.getch()
print('goodbye!')
snakebod = [randompos(mainwin)]
snakebod.append(snakebod[0])
mainwin.clearbuffer(" ")
drawborders(mainwin)
drawsnake(mainwin, snakebod)
mainwin.drawscreen()
kb = KBHit()
dir = [0, 0]
dtime = time.time()
while True:
fdir = [0, 0]
if kb.kbhit(): # keyboard
c = kb.getch()
if ord(c) == 27: # ESC
break
if ord(c) == 119: #W
fdir = [0, -1]
elif ord(c) == 97: #A
fdir = [-1, 0]
elif ord(c) == 115: #S
fdir = [0, 1]
elif ord(c) == 100: #D
fdir = [1, 0]
if fdir != [0, 0]:
if [snakebod[0][0] + fdir[0],
# shuffle_in_unison_scary(X_train, Y_train)
prevLoss = 0.0
maxModel = None
stop = False
for args in itertools.product(nb_hiddens, drop1s, drop2s, drop3s, nb_filters, nb_pools, nb_rows, nb_columns):
print("Model: ", args)
hist, model = get_model_and_score(X_train, Y_train, *args)
# most recent loss hist.history["loss"][-1]
r, rmse, _ = assess_model(model, X_validate, Y_validate)
print("Model r: ", r)
print("Model rmse: ", rmse)
if abs(r[0]) > prevLoss:
prevLoss = abs(r[0])
maxModel = model
while kb.kbhit():
try:
if "q" in kb.getch():
print("quiting due to user pressing q")
stop = True
except UnicodeDecodeError:
pass
if stop:
break
del X_train
X_test = np.array(X_test)
Y_test_norm = np.array(list(map(normalize_bpm, Y_test)))
beams = []
beam_places_list = [np.array(range(board.get_bg_size()[0]+6,board.get_bg_size()[1]-board.get_window_size()[1],6)),np.array(range(din.get_ceil_ground()[1]+20,din.get_ceil_ground()[0]-10,6))]
for i in range(num_beams):
x = (int(np.random.choice(beam_places_list[0])),int(np.random.choice(beam_places_list[1])))
if x not in coins and x[0]:
beams.append(Beam(x[0],x[1],np.random.choice([7,8,12]),np.random.choice([0,1,2])))
coins = sorted(coins,key = lambda x: x.get_pos()[0])
magnet = Magnet(np.random.randint(board.get_window_size()[0]+10,board.get_bg_size()[1]-board.get_window_size()[1]))
dragon = Dragon(din,board)
# board.set_current_pos(board.get_bg_size()[1] - board.get_window_size()[1])
kb.getch()
while dragon.get_ended() == 0 and din.get_lives() > 0:
board.update()
if kb.kbhit():
c = kb.getch()
if c == 'q':
break
if c == 'k':
board.speed_up()
if c == 'j':
board.speed_down()
din_ret = din.update(c)
if c == 's':
bullets.append(din_ret)
else:
din.update()
for bullet in bullets:
bullet.update()
direction = new_direction
#m.move(direction, speed=70)
else:
#m.course_correct()
pass
dist = us_dist(15)
if (dist < dist_to_stop):
print("too close, stop")
m.stop()
break
else: # we could not find the target - stop and wait
m.stop
# clear the stream in preparation for the next frame
rawCapture.truncate(0)
i = i+1
time.sleep(0.1) #allow the system some time - don't hog the CPU
# if the `q` key was pressed, break from the loop
if ch == ord('q'):
break
if (kb.kbhit()):
c = kb.getch()
if (c == 'q'):
break
if ( (time.clock() - tstart) > max_time_to_run):
break
print("fps: ", i / (time.time() - tstart), i, time.time() - tstart)
m.stop()
cv2.destroyAllWindows()
print("out")
import os, numpy, sys, time
from levels import Level1, Moderate, Easy, Difficult, FinalLevel
# os.system('afplay ' + 'barbie.mp3 &')
keyboard = KBHit()
lives = 5
score = 0
while lives > 0:
# level 1
level = Easy()
level.placeBricks()
while True:
level.screen.time += level.ball.waitTime
time.sleep(level.ball.waitTime)
if keyboard.kbhit():
inp = keyboard.getch()
if inp == 'q':
sys.exit()
elif inp == 'a' or inp == 'd':
level.paddle.move(inp)
elif inp == ' ':
level.ball.launch()
elif inp == 't':
break
keyboard.flush()
level.ball.move()
if level.ball.gameOver == True:
break
os.system("clear")
