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()
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()
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 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_your_turn(self):
self.goto_base_position()
TrajectoryPlayer(self.reachy, moves['your-turn']).play(wait=True)
self.goto_rest_position()
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()
wait=True,
interpolation_mode="minjerk",
starting_point="goal_position",
)
else:
'''
if True:
move = moves[selection]
if np.ndim(move[move.files[0]]) == 0: # Position
reachy.goto(
goal_positions=move,
duration=1,
wait=True,
interpolation_mode="minjerk",
starting_point="goal_position",
)
else:
j = {
m: j for j, m in zip(np.array(list(move.values()))[:, 0], list(move.keys()))
}
reachy.goto(
goal_positions=j,
duration=0.5,
wait=True,
interpolation_mode="minjerk",
starting_point="goal_position",
)
TrajectoryPlayer(reachy, move).play(wait=True)