class LimitedTictactoePlayground(object):
def __init__(self):
print("Creating the playground")
self.reachy = Reachy(right_arm=parts.RightArm(
io=io_setting,
hand="force_gripper",
))
self.pawn_played = 0
def setup(self):
print("Setup the playground")
self.goto_rest_position()
def __enter__(self):
return self
def __exit__(self, *exc):
print("Closing the playground")
self.reachy.close()
# Playground and game functions
def shuffle_board(self):
self.goto_base_position()
# self.reachy.head.look_at(0.5, 0, -0.4, duration=1, wait=False)
m = moves['shuffle-board'] # Trevor change
j = {
m: j
for j, m in zip(np.array(list(m.values()))[:, 0], list(m.keys()))
}
self.goto_position(j, duration=0.5, wait=True)
TrajectoryPlayer(self.reachy, m).play(wait=True)
self.goto_rest_position()
# self.reachy.head.look_at(1, 0, 0, duration=1, wait=True)
def play_pawn(self, grab_index, box_index):
# Goto base position
self.goto_base_position()
if grab_index >= 4:
self.goto_position(
moves["grab_3"],
duration=1,
wait=True,
)
# Grab the pawn at grab_index
self.goto_position(
moves[f"grab_{grab_index}"],
duration=1,
wait=True,
)
self.goto_position(moves["grip_pawn"], duration=0.5,
wait=True) # Trevor change
if grab_index >= 4:
self.reachy.goto(
{
"right_arm.shoulder_pitch":
self.reachy.right_arm.shoulder_pitch.goal_position + 10,
"right_arm.elbow_pitch":
self.reachy.right_arm.elbow_pitch.goal_position - 30,
},
duration=1,
wait=True,
interpolation_mode="minjerk",
starting_point="goal_position",
)
# Lift it
self.goto_position(
moves["lift"],
duration=1,
wait=True,
)
# self.reachy.head.look_at(0.5, 0, -0.35, duration=0.5, wait=False)
time.sleep(0.1)
# Put it in box_index
put = moves[f"put_{box_index}"] # Trevor change
j = {
m: j
for j, m in zip(
np.array(list(put.values()))[:, 0], list(put.keys()))
}
self.goto_position(j, duration=0.5, wait=True)
TrajectoryPlayer(self.reachy, put).play(wait=True)
self.reachy.right_arm.hand.open()
# Go back to rest position
self.goto_position(
moves[f"back_{box_index}_upright"],
duration=1,
wait=True,
)
# self.reachy.head.look_at(1, 0, 0, duration=1, wait=False)
if box_index in (8, 9):
self.goto_position(
moves["back_to_back"],
duration=1,
wait=True,
)
self.goto_position(
moves["back_rest"],
duration=2,
wait=True,
)
self.goto_rest_position()
def run_my_turn(self):
self.goto_base_position()
m = moves['my-turn'] # Trevor change
j = {
m: j
for j, m in zip(np.array(list(m.values()))[:, 0], list(m.keys()))
}
self.goto_position(j, duration=0.5, wait=True)
TrajectoryPlayer(self.reachy, m).play(wait=True)
self.goto_rest_position()
def run_your_turn(self):
self.goto_base_position()
m = moves['your-turn'] # Trevor change
j = {
m: j
for j, m in zip(np.array(list(m.values()))[:, 0], list(m.keys()))
}
self.goto_position(j, duration=0.5, wait=True)
TrajectoryPlayer(self.reachy, m).play(wait=True)
self.goto_rest_position()
# Robot lower-level control functions
def goto_position(self, goal_positions, duration, wait):
self.reachy.goto(
goal_positions=goal_positions,
duration=duration,
wait=wait,
interpolation_mode="minjerk",
starting_point="goal_position",
)
def goto_base_position(self, duration=2.0):
for m in self.reachy.right_arm.motors:
m.compliant = False
time.sleep(0.1)
self.reachy.right_arm.shoulder_pitch.torque_limit = 75
self.reachy.right_arm.elbow_pitch.torque_limit = 75
time.sleep(0.1)
self.goto_position(moves["base_pos"], duration,
wait=True) # Trevor change
def goto_rest_position(self, duration=2.0):
# FIXME: Why is it needed?
time.sleep(0.1)
self.goto_base_position(0.6 * duration)
time.sleep(0.1)
self.goto_position(moves["rest_pos"], 0.4 * duration,
wait=True) # Trevor change
time.sleep(0.1)
self.reachy.right_arm.shoulder_pitch.torque_limit = 0
self.reachy.right_arm.elbow_pitch.torque_limit = 0
time.sleep(0.25)
for m in self.reachy.right_arm.motors:
if m.name != "right_arm.shoulder_pitch":
m.compliant = True
time.sleep(0.25)
def need_cooldown(self):
motor_temperature = np.array(
[m.temperature for m in self.reachy.motors])
temperatures = {}
temperatures.update(
{m.name: m.temperature
for m in self.reachy.motors})
print(f"Checking motor temperatures: {temperatures}")
return np.any(motor_temperature > 50)
def wait_for_cooldown(self):
self.goto_rest_position()
# self.reachy.head.look_at(0.5, 0, -0.65, duration=1.25, wait=True)
# self.reachy.head.compliant = True
while True:
motor_temperature = np.array(
[m.temperature for m in self.reachy.motors])
temperatures = {}
temperatures.update(
{m.name: m.temperature
for m in self.reachy.motors})
print(f"Motors cooling down... {temperatures}")
if np.all(motor_temperature < 45):
break
time.sleep(30)
def enter_sleep_mode(self):
# self.reachy.head.look_at(0.5, 0, -0.65, duration=1.25, wait=True)
# self.reachy.head.compliant = True
self._idle_running = Event()
self._idle_running.set()
def _idle():
while self._idle_running.is_set():
time.sleep(0.01)
self._idle_t = Thread(target=_idle)
self._idle_t.start()
def leave_sleep_mode(self):
# self.reachy.head.compliant = False
time.sleep(0.1)
# self.reachy.head.look_at(1, 0, 0, duration=1, wait=True)
self._idle_running.clear()
self._idle_t.join()
class TictactoePlayground(object):
def __init__(self):
logger.info('Creating the playground')
self.reachy = Reachy(
right_arm=RightArm(
io='/dev/ttyUSB*',
hand='force_gripper',
),
head=Head(io='/dev/ttyUSB*', ),
)
self.pawn_played = 0
def setup(self):
logger.info('Setup the playground')
for antenna in self.reachy.head.motors:
antenna.compliant = False
antenna.goto(
goal_position=0,
duration=2,
interpolation_mode='minjerk',
)
self.goto_rest_position()
def __enter__(self):
return self
def __exit__(self, *exc):
logger.info('Closing the playground', extra={
'exc': exc,
})
self.reachy.close()
# Playground and game functions
def reset(self):
logger.info('Resetting the playground')
self.pawn_played = 0
empty_board = np.zeros((3, 3), dtype=np.uint8).flatten()
return empty_board
def is_ready(self, board):
return np.sum(board) == 0
def random_look(self):
dy = 0.4
y = np.random.rand() * dy - (dy / 2)
dz = 0.75
z = np.random.rand() * dz - 0.5
self.reachy.head.look_at(0.5, y, z, duration=1.5, wait=True)
def run_random_idle_behavior(self):
logger.info('Reachy is playing a random idle behavior')
time.sleep(2)
def coin_flip(self):
coin = np.random.rand() > 0.5
logger.info(
'Coin flip',
extra={
'first player': 'reachy' if coin else 'human',
},
)
return coin
def analyze_board(self):
for disk in self.reachy.head.neck.disks:
disk.compliant = False
time.sleep(0.1)
self.reachy.head.look_at(0.5, 0, z=-0.6, duration=1, wait=True)
time.sleep(0.2)
# Wait an image from the camera
self.wait_for_img()
success, img = self.reachy.head.right_camera.read()
# TEMP:
import cv2 as cv
i = np.random.randint(1000)
path = f'/tmp/snap.{i}.jpg'
cv.imwrite(path, img)
logger.info(
'Getting an image from camera',
extra={
'img_path': path,
'disks': [d.rot_position for d in self.reachy.head.neck.disks],
},
)
if not is_board_valid(img):
self.reachy.head.compliant = False
time.sleep(0.1)
self.reachy.head.look_at(1, 0, 0, duration=0.75, wait=True)
return
tic = time.time()
success, img = self.reachy.head.right_camera.read()
board, _ = get_board_configuration(img)
# TEMP
logger.info(
'Board analyzed',
extra={
'board': board,
'img_path': path,
},
)
self.reachy.head.compliant = False
time.sleep(0.1)
self.reachy.head.look_at(1, 0, 0, duration=0.75, wait=True)
return board.flatten()
def incoherent_board_detected(self, board):
nb_cubes = len(np.where(board == piece2id['cube'])[0])
nb_cylinders = len(np.where(board == piece2id['cylinder'])[0])
if abs(nb_cubes - nb_cylinders) <= 1:
return False
logger.warning('Incoherent board detected',
extra={
'current_board': board,
})
return True
def cheating_detected(self, board, last_board, reachy_turn):
# last is just after the robot played
delta = board - last_board
# Nothing changed
if np.all(delta == 0):
return False
# A single cube was added
if len(np.where(delta == piece2id['cube'])[0]) == 1:
return False
# A single cylinder was added
if len(np.where(delta == piece2id['cylinder'])[0]) == 1:
# If the human added a cylinder
if not reachy_turn:
return True
return False
logger.warning('Cheating detected',
extra={
'last_board': last_board,
'current_board': board,
})
return True
def shuffle_board(self):
def ears_no():
d = 3
f = 2
time.sleep(2.5)
t = np.linspace(0, d, d * 100)
p = 25 + 25 * np.sin(2 * np.pi * f * t)
for pp in p:
self.reachy.head.left_antenna.goal_position = pp
time.sleep(0.01)
t = Thread(target=ears_no)
t.start()
self.goto_base_position()
self.reachy.head.look_at(0.5, 0, -0.4, duration=1, wait=False)
TrajectoryPlayer(self.reachy, moves['shuffle-board']).play(wait=True)
self.goto_rest_position()
self.reachy.head.look_at(1, 0, 0, duration=1, wait=True)
t.join()
def choose_next_action(self, board):
actions = value_actions(board)
# If empty board starts with a random actions for diversity
if np.all(board == 0):
while True:
i = np.random.randint(0, 9)
a, _ = actions[i]
if a != 8:
break
elif np.sum(board) == piece2id['cube']:
a, _ = actions[0]
if a == 8:
i = 1
else:
i = 0
else:
i = 0
best_action, value = actions[i]
logger.info(
'Selecting Reachy next action',
extra={
'board': board,
'actions': actions,
'selected action': best_action,
},
)
return best_action, value
def play(self, action, actual_board):
board = actual_board.copy()
self.play_pawn(
grab_index=self.pawn_played + 1,
box_index=action + 1,
)
self.pawn_played += 1
board[action] = piece2id['cylinder']
logger.info(
'Reachy playing pawn',
extra={
'board-before': actual_board,
'board-after': board,
'action': action + 1,
'pawn_played': self.pawn_played + 1,
},
)
return board
def play_pawn(self, grab_index, box_index):
self.reachy.head.look_at(
0.3,
-0.3,
-0.3,
duration=0.85,
wait=False,
)
# Goto base position
self.goto_base_position()
if grab_index >= 4:
self.goto_position(
moves['grab_3'],
duration=1,
wait=True,
)
# Grab the pawn at grab_index
self.goto_position(
moves[f'grab_{grab_index}'],
duration=1,
wait=True,
)
self.reachy.right_arm.hand.close()
self.reachy.head.left_antenna.goto(45, 1, interpolation_mode='minjerk')
self.reachy.head.right_antenna.goto(-45,
1,
interpolation_mode='minjerk')
if grab_index >= 4:
self.reachy.goto(
{
'right_arm.shoulder_pitch':
self.reachy.right_arm.shoulder_pitch.goal_position + 10,
'right_arm.elbow_pitch':
self.reachy.right_arm.elbow_pitch.goal_position - 30,
},
duration=1,
wait=True,
interpolation_mode='minjerk',
starting_point='goal_position',
)
# Lift it
self.goto_position(
moves['lift'],
duration=1,
wait=True,
)
self.reachy.head.look_at(0.5, 0, -0.35, duration=0.5, wait=False)
time.sleep(0.1)
# Put it in box_index
put = moves[f'put_{box_index}_smooth_10_kp']
j = {
m: j
for j, m in zip(
np.array(list(put.values()))[:, 0], list(put.keys()))
}
self.goto_position(j, duration=0.5, wait=True)
TrajectoryPlayer(self.reachy, put).play(wait=True)
self.reachy.right_arm.hand.open()
# Go back to rest position
self.goto_position(
moves[f'back_{box_index}_upright'],
duration=1,
wait=True,
)
self.reachy.head.left_antenna.goto(0,
0.2,
interpolation_mode='minjerk')
self.reachy.head.right_antenna.goto(0,
0.2,
interpolation_mode='minjerk')
self.reachy.head.look_at(1, 0, 0, duration=1, wait=False)
if box_index in (8, 9):
self.goto_position(
moves['back_to_back'],
duration=1,
wait=True,
)
self.goto_position(
moves['back_rest'],
duration=2,
wait=True,
)
self.goto_rest_position()
def is_final(self, board):
winner = self.get_winner(board)
if winner in ('robot', 'human'):
return True
else:
return 0 not in board
def has_human_played(self, current_board, last_board):
cube = piece2id['cube']
return (np.any(current_board != last_board)
and np.sum(current_board == cube) > np.sum(last_board == cube))
def get_winner(self, board):
win_configurations = (
(0, 1, 2),
(3, 4, 5),
(6, 7, 8),
(0, 3, 6),
(1, 4, 7),
(2, 5, 8),
(0, 4, 8),
(2, 4, 6),
)
for c in win_configurations:
trio = set(board[i] for i in c)
for id in id2piece.keys():
if trio == set([id]):
winner = piece2player[id2piece[id]]
if winner in ('robot', 'human'):
return winner
return 'nobody'
def run_celebration(self):
logger.info('Reachy is playing its win behavior')
behavior.happy(self.reachy)
def run_draw_behavior(self):
logger.info('Reachy is playing its draw behavior')
behavior.surprise(self.reachy)
def run_defeat_behavior(self):
logger.info('Reachy is playing its defeat behavior')
behavior.sad(self.reachy)
def run_my_turn(self):
self.goto_base_position()
TrajectoryPlayer(self.reachy, moves['my-turn']).play(wait=True)
self.goto_rest_position()
def run_your_turn(self):
self.goto_base_position()
TrajectoryPlayer(self.reachy, moves['your-turn']).play(wait=True)
self.goto_rest_position()
# Robot lower-level control functions
def goto_position(self, goal_positions, duration, wait):
self.reachy.goto(
goal_positions=goal_positions,
duration=duration,
wait=wait,
interpolation_mode='minjerk',
starting_point='goal_position',
)
def goto_base_position(self, duration=2.0):
for m in self.reachy.right_arm.motors:
m.compliant = False
time.sleep(0.1)
self.reachy.right_arm.shoulder_pitch.torque_limit = 75
self.reachy.right_arm.elbow_pitch.torque_limit = 75
time.sleep(0.1)
self.goto_position(base_pos, duration, wait=True)
def goto_rest_position(self, duration=2.0):
# FIXME: Why is it needed?
time.sleep(0.1)
self.goto_base_position(0.6 * duration)
time.sleep(0.1)
self.goto_position(rest_pos, 0.4 * duration, wait=True)
time.sleep(0.1)
self.reachy.right_arm.shoulder_pitch.torque_limit = 0
self.reachy.right_arm.elbow_pitch.torque_limit = 0
time.sleep(0.25)
for m in self.reachy.right_arm.motors:
if m.name != 'right_arm.shoulder_pitch':
m.compliant = True
time.sleep(0.25)
def wait_for_img(self):
start = time.time()
while time.time() - start <= 30:
success, img = self.reachy.head.right_camera.read()
if img != []:
return
logger.warning('No image received for 30 sec, going to reboot.')
os.system('sudo reboot')
def need_cooldown(self):
motor_temperature = np.array(
[m.temperature for m in self.reachy.motors])
orbita_temperature = np.array(
[d.temperature for d in self.reachy.head.neck.disks])
temperatures = {}
temperatures.update(
{m.name: m.temperature
for m in self.reachy.motors})
temperatures.update(
{d.alias: d.temperature
for d in self.reachy.head.neck.disks})
logger.info('Checking Reachy motors temperature',
extra={'temperatures': temperatures})
return np.any(motor_temperature > 50) or np.any(
orbita_temperature > 45)
def wait_for_cooldown(self):
self.goto_rest_position()
self.reachy.head.look_at(0.5, 0, -0.65, duration=1.25, wait=True)
self.reachy.head.compliant = True
while True:
motor_temperature = np.array(
[m.temperature for m in self.reachy.motors])
orbita_temperature = np.array(
[d.temperature for d in self.reachy.head.neck.disks])
temperatures = {}
temperatures.update(
{m.name: m.temperature
for m in self.reachy.motors})
temperatures.update(
{d.name: d.temperature
for d in self.reachy.head.neck.disks})
logger.warning(
'Motors cooling down...',
extra={'temperatures': temperatures},
)
if np.all(motor_temperature < 45) and np.all(
orbita_temperature < 40):
break
time.sleep(30)
def enter_sleep_mode(self):
self.reachy.head.look_at(0.5, 0, -0.65, duration=1.25, wait=True)
self.reachy.head.compliant = True
self._idle_running = Event()
self._idle_running.set()
def _idle():
f = 0.15
amp = 30
offset = 30
while self._idle_running.is_set():
p = offset + amp * np.sin(2 * np.pi * f * time.time())
self.reachy.head.left_antenna.goal_position = p
self.reachy.head.right_antenna.goal_position = -p
time.sleep(0.01)
self._idle_t = Thread(target=_idle)
self._idle_t.start()
def leave_sleep_mode(self):
self.reachy.head.compliant = False
time.sleep(0.1)
self.reachy.head.look_at(1, 0, 0, duration=1, wait=True)
self._idle_running.clear()
self._idle_t.join()
