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 __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 __init__(self,
node_name="speechsynthesis",
host="127.0.0.1",
port=1883,
username=None,
password=None,
subscribe_dict={},
run_sleep=0.1,
env="dev"):
super().__init__(node_name, host, port, username, password,
subscribe_dict, run_sleep)
curr_platform = get_platform()
if env == "dev":
self.io = "ws"
elif env == "reachy":
self.io = '/dev/ttyUSB*'
parts.Head = patch_head(parts.Head)
parts.RightArm = patch_right_arm(parts.RightArm)
parts.arm.RightForceGripper = patch_force_gripper(
parts.arm.RightForceGripper)
else:
raise Exception("Invalid env: {}".format(env))
self.reachy = Reachy(
head=parts.Head(io=self.io),
right_arm=parts.RightArm(io=self.io, hand='force_gripper'),
)
self.running = True
command_input = input("press enter after connecting to Reachy>")
self.initialize()
self.add_subscribe('+/body/init', self.handle_initialize)
self.add_subscribe('+/body/shutdown', self.handle_shutdown)
self.add_subscribe('+/body/stop', self.handle_force_stop)
self.add_subscribe('+/body/right_arm/wave', self.handle_wave_arm)
self.add_subscribe('+/body/head/antenna/wiggle',
self.handle_wiggle_antennas)
self.add_subscribe('+/body/head/antenna/zero',
self.move_antennas_to_zero)
self.add_subscribe('+/body/right_arm/zero',
self.handle_wiggle_antennas)
def __init__(self):
config_path = '/home/seb/reachy/software/reachy/configuration/reachy-force-gripper.json'
self.reachy = Reachy(config=config_path)
# self.reachy = Reachy()
# self.reachy.force_gripper.offset = 4
# self.reachy.force_gripper.scale = 10000
# Load tic tac to position
with open("tictacto.json", "r") as f:
self.all_positions = json.load(f)
def from_vrep(scene, robot, **extra):
robot, id = extract_robot(robot)
extra['id'] = id
if robot == 'reachy':
return Reachy(simulator='vrep', scene=scene, **extra)
if robot == 'leachy':
return Leachy(simulator='vrep', scene=scene, **extra)
raise ValueError('Robot must either "reachy" or "leachy"!')
def main():
reachy = Reachy()
for motor in reachy.motors:
motor.compliant = True
while True:
print(30 * '\n')
for motor in reachy.motors:
print(
f"The motor \"{motor.name}\" is in position: {motor.present_position}"
)
time.sleep(1)
def main():
Config.load_conf()
config = Config.get_dict()
reachy = Reachy()
for motor in reachy.motors:
motor.compliant = True
if config["record"]:
# First part of the script : the user can move the arm and hit return to save the arm position
all_positions = []
action = input("> ")
while action != "save":
# Save the position of the motors
position = {}
for motor in reachy.motors:
print(
f"The motor \"{motor.name}\" is currently in position: {motor.present_position}"
)
position[motor.name] = motor.present_position
all_positions.append(position)
action = input("> ")
# Save the list of position in a file
with open(config["file_path"], "w") as f:
json.dump(all_positions, f)
else:
# If this first part was not executed, then we need to read the json file to load the key points
with open(config["file_path"], "r") as f:
all_positions = json.load(f)
# Move the arm
for motor in reachy.motors:
motor.compliant = False
for position in all_positions:
# Move to position
for motor in reachy.motors:
motor.goal_position = position[motor.name]
# Wait 2 s
time.sleep(2)
for motor in reachy.motors:
motor.compliant = True
try:
from . import (assistant_helpers, audio_helpers, browser_helpers,
device_helpers)
except (SystemError, ImportError):
import assistant_helpers
import audio_helpers
import browser_helpers
import device_helpers
from azure.cognitiveservices.speech import AudioDataStream, SpeechConfig, SpeechSynthesizer, SpeechSynthesisOutputFormat
from azure.cognitiveservices.speech.audio import AudioOutputConfig
from reachy import parts, Reachy
robot = Reachy(
right_arm=parts.RightArm(io='ws', hand='force_gripper'),
left_arm=parts.LeftArm(io='ws', hand='force_gripper'),
)
engine = pyttsx3.init()
robot.left_arm.shoulder_roll.goal_position = 0
robot.left_arm.arm_yaw.goal_position = 0
robot.left_arm.elbow_pitch.goal_position = 0
robot.left_arm.hand.forearm_yaw.goal_position = 0
speech_config = SpeechConfig(subscription="subscriptionkey",
region="westeurope")
audio_config = AudioOutputConfig(use_default_speaker=True)
synthesizer = SpeechSynthesizer(speech_config=speech_config,
audio_config=audio_config)
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()
%matplotlib notebook
import time
import cv2 as cv
import numpy as np
from matplotlib import pyplot as plt
from reachy import Reachy, parts
reachy = Reachy(
#head=parts.Head(io='/dev/ttyUSB*'),
head=parts.Head(io='ws'),
)
for m in reachy.head.motors:
m.compliant = False
for m in reachy.head.motors:
m.goal_position = -45
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()
"right_arm.hand.wrist_pitch": -60,
"right_arm.hand.wrist_roll": 0,
}
base_pos = {
"right_arm.shoulder_pitch": 60,
"right_arm.shoulder_roll": -15,
"right_arm.arm_yaw": 0,
"right_arm.elbow_pitch": -95,
"right_arm.hand.forearm_yaw": -15,
"right_arm.hand.wrist_pitch": -50,
"right_arm.hand.wrist_roll": 0,
"right_arm.hand.gripper": -45,
}
reachy = Reachy(right_arm=parts.RightArm(io="ws", hand="force_gripper"))
input("Connect the Unity simulator, then press Enter to continue.")
while True:
selection = input("Name a move: ")
'''
if selection == 'rest_pos':
reachy.goto(
goal_positions=rest_pos,
duration=1,
wait=True,
interpolation_mode="minjerk",
starting_point="goal_position",
)
elif selection == 'base_pos':
return forceGripper
parts.arm.RightForceGripper = patch_force_gripper(parts.arm.RightForceGripper)
io_setting = None
while not io_setting:
input_answer = input("Is this a simulated Reachy? y/n? ")
if input_answer == "y" or input_answer == "Y":
io_setting = "ws"
elif input_answer == "n" or input_answer == "N":
io_setting = "/dev/ttyUSB*"
try:
reachy = Reachy(right_arm=parts.RightArm(io=io_setting, hand="force_gripper"))
except:
traceback.print_exc()
exit("Exception when initializing Reachy")
if io_setting == "ws":
input("Connect the Unity simulator, then press Enter to continue.")
for m in reachy.right_arm.motors:
print(f"Motor found: {m.name} - pos:{m.present_position}")
m.compliant = True
stop_loop = False
while not stop_loop:
print("Ready to record!")
%matplotlib notebook
import time
import numpy as np
from matplotlib import pyplot as plt
from reachy import Reachy, parts
#Change this for USB port
reachy = Reachy(
#right_arm=parts.RightArm(io='/dev/ttyUSB*', hand='force_gripper'),
right_arm=parts.RightArm(io='ws', hand='force_gripper'),
)
for m in reachy.right_arm.motors:
m.compliant = False
#Wave Frame 1, bring arm up
reachy.goto({
'right_arm.shoulder_pitch': -20,
'right_arm.shoulder_roll': -10,
'right_arm.arm_yaw': -10,
'right_arm.elbow_pitch': -120,
'right_arm.hand.forearm_yaw': 0,
'right_arm.hand.wrist_pitch': 0,
'right_arm.hand.wrist_roll': 0,
'right_arm.hand.gripper': 0,
}, duration=2, wait=True)
#Wave Frame 2 (Hand Wave)
def setUp(self):
self.reachy = Reachy(
left_arm=parts.LeftArm(io='', hand='force_gripper'),
right_arm=parts.RightArm(io='', hand='force_gripper'),
)
class Body(NodeBase):
def __init__(self,
node_name="speechsynthesis",
host="127.0.0.1",
port=1883,
username=None,
password=None,
subscribe_dict={},
run_sleep=0.1,
env="dev"):
super().__init__(node_name, host, port, username, password,
subscribe_dict, run_sleep)
curr_platform = get_platform()
if env == "dev":
self.io = "ws"
elif env == "reachy":
self.io = '/dev/ttyUSB*'
parts.Head = patch_head(parts.Head)
parts.RightArm = patch_right_arm(parts.RightArm)
parts.arm.RightForceGripper = patch_force_gripper(
parts.arm.RightForceGripper)
else:
raise Exception("Invalid env: {}".format(env))
self.reachy = Reachy(
head=parts.Head(io=self.io),
right_arm=parts.RightArm(io=self.io, hand='force_gripper'),
)
self.running = True
command_input = input("press enter after connecting to Reachy>")
self.initialize()
self.add_subscribe('+/body/init', self.handle_initialize)
self.add_subscribe('+/body/shutdown', self.handle_shutdown)
self.add_subscribe('+/body/stop', self.handle_force_stop)
self.add_subscribe('+/body/right_arm/wave', self.handle_wave_arm)
self.add_subscribe('+/body/head/antenna/wiggle',
self.handle_wiggle_antennas)
self.add_subscribe('+/body/head/antenna/zero',
self.move_antennas_to_zero)
self.add_subscribe('+/body/right_arm/zero',
self.handle_wiggle_antennas)
def initialize(self):
# move to the stop state for now
self.state = ActionQueue.STOPPED
self.set_compliance_all(False)
# move to zero
self.move_all_to_zero()
# turn compliance back on
self.set_compliance_all(True)
# turn the fans on for the arm
self.turn_on_fans()
# only set the status to RUNNING after everything has been initialized
self.state = ActionQueue.RUNNING
self.state = ActionQueue.IDLE
self.action_queue = ActionQueue(self)
self.action_queue.run()
self.state = ActionQueue.RUNNING
"""
def node_init(self):
self.initialize()
super().node_init()
"""
def handle_initialize(self, client=None, userdata=None, message=None):
self.initialize()
def shutdown(self, client=None, userdata=None, message=None):
self.state = ActionQueue.STOPPED
# set to status to STOPPED first
self.clear_action_queue()
self.set_compliance_all(False)
# move to zero
self.move_all_to_zero()
# turn compliance back on
self.set_compliance_all(True)
# turn the fans off for the arm
self.turn_off_fans()
def handle_shutdown(self, client=None, userdata=None, message=None):
self.shutdown()
def force_stop(self):
self.state = ActionQueue.STOPPED
# this should only be done in an emergency
self.set_right_arm_compliance(True)
self.set_head_compliance(True)
def handle_force_stop(self, client=None, userdata=None, message=None):
self.force_stop()
def reset(self):
self.state = ActionQueue.IDLE
self.action_queue.clear()
def turn_on_fans(self):
self.reachy.right_arm.shoulder_fan.on()
self.reachy.right_arm.elbow_fan.on()
self.reachy.right_arm.wrist_fan.on()
def turn_off_fans(self):
self.reachy.right_arm.shoulder_fan.off()
self.reachy.right_arm.elbow_fan.off()
self.reachy.right_arm.wrist_fan.off()
def set_head_compliance(self, compliance):
for m in self.reachy.head.motors:
m.compliant = compliance
def set_right_arm_compliance(self, compliance):
for m in self.reachy.right_arm.motors:
m.compliant = compliance
def set_compliance_all(self, compliance):
self.set_head_compliance(compliance)
self.set_right_arm_compliance(compliance)
def move_antennas_to_zero(self, client=None, userdata=None, message=None):
print("###move_antennas_to_zero")
self.set_head_compliance(False)
for m in self.reachy.head.motors:
m.goal_position = 0
time.sleep(1)
#self.set_head_compliance(True)
def move_right_arm_to_zero(self,
client=None,
userdata=None,
message=None,
set_compliance=False):
if set_compliance:
self.set_right_arm_compliance(False)
self.reachy.goto(
{
'right_arm.shoulder_pitch': 0,
'right_arm.shoulder_roll': 0,
'right_arm.arm_yaw': 0,
'right_arm.elbow_pitch': 0,
'right_arm.hand.forearm_yaw': 0,
'right_arm.hand.wrist_pitch': 0,
'right_arm.hand.wrist_roll': 0,
'right_arm.hand.gripper': 0,
},
duration=3,
wait=True)
if set_compliance:
self.set_right_arm_compliance(True)
def move_all_to_zero(self):
self.move_antennas_to_zero()
self.move_right_arm_to_zero()
def say(self, msg):
say_msg = SayMessage(msg)
payload = say_msg.to_json()
print(payload)
self.publish("body/say/request", payload=payload)
def make_wave_arm(self, say_msg=None):
def wave_arm():
print("###wave_arm - 1")
self.set_right_arm_compliance(False)
zero_posR = {
'right_arm.shoulder_pitch': 0,
'right_arm.shoulder_roll': 0,
'right_arm.arm_yaw': 0,
'right_arm.elbow_pitch': 0,
'right_arm.hand.forearm_yaw': 0,
'right_arm.hand.wrist_pitch': 0,
'right_arm.hand.wrist_roll': 0,
'right_arm.hand.gripper': 0,
}
# Moving arm and hand into position
pos_RA = {
'right_arm.shoulder_pitch': 0, #-20
'right_arm.shoulder_roll': -45, # -10
'right_arm.arm_yaw': -70, # -10
'right_arm.elbow_pitch': -115, # -120
'right_arm.hand.forearm_yaw': -60, # 0
'right_arm.hand.wrist_pitch': 0, # 0
'right_arm.hand.wrist_roll': -30, # 0
'right_arm.hand.gripper': 0, # 0
}
# Loop Hand Wave
pos_RB = {
'right_arm.shoulder_pitch': 0,
'right_arm.shoulder_roll': -15,
'right_arm.arm_yaw': -70,
'right_arm.elbow_pitch': -115,
'right_arm.hand.forearm_yaw': -60,
'right_arm.hand.wrist_pitch': 0,
'right_arm.hand.wrist_roll': 30, # 40
'right_arm.hand.gripper': 0,
}
pos_RC = {
'right_arm.shoulder_pitch': 0,
'right_arm.shoulder_roll': -45,
'right_arm.arm_yaw': -70,
'right_arm.elbow_pitch': -115,
'right_arm.hand.forearm_yaw': -60,
'right_arm.hand.wrist_pitch': 0,
'right_arm.hand.wrist_roll': -30, # -40
'right_arm.hand.gripper': 0,
}
print("###wave_arm - 2")
# rehome start and finish, with loop
self.reachy.head.left_antenna.goto(0, duration=4, wait=False)
self.reachy.head.right_antenna.goto(0, duration=4, wait=False)
self.reachy.goto(goal_positions=zero_posR, duration=2, wait=True)
time.sleep(2)
# TODO: ok... this is kind of hacky. The say message
# should really be coming from the main controller.
# Just do it here so it's more synchronized with the
# wave.
if say_msg is not None:
self.say(say_msg)
print("###wave_arm - 3")
self.reachy.goto(goal_positions=pos_RA, duration=2.5, wait=True)
self.reachy.head.left_antenna.goto(
-45, duration=4, wait=False
) # depends on actual calibration - may need opp sign
for m in range(3):
self.reachy.goto(goal_positions=pos_RB, duration=1, wait=True)
self.reachy.goto(goal_positions=pos_RC, duration=1, wait=True)
time.sleep(1)
print("###wave_arm - 4")
self.reachy.head.left_antenna.goto(0, duration=4, wait=False)
self.reachy.goto(goal_positions=zero_posR, duration=2, wait=True)
time.sleep(2)
print("###wave_arm - 5")
self.set_right_arm_compliance(True)
return wave_arm
def handle_wave_arm(self, client=None, userdata=None, message=None):
print("###handle_wave_arm - 1")
try:
_message = str(message.payload.decode("utf-8"))
print("###handle_wave_arm - 2: ", _message)
right_arm_msg = RightArmMessage.from_json(_message)
self.action_queue.add(self.make_wave_arm(right_arm_msg.msg))
except Exception as e:
print("###handle_wave_arm - 3 - e: ", e)
self.action_queue.add(self.make_wave_arm())
"""
def make_wiggle_antennas(self):
def wiggle_antennas():
print("###adding wiggle to queue")
self.set_head_compliance(False)
print("###wiggle - 1")
for m in self.reachy.head.motors:
m.goal_position = 0
print("###wiggle - 2")
for m in self.reachy.head.motors:
m.goal_position = 45
print("###wiggle - 3")
for m in self.reachy.head.motors:
m.goal_position = 0
print("###wiggle - 4")
t = np.linspace(0, 10, 1000)
pos = 30 * np.sin(2 * np.pi * 0.5 * t)
print("###wiggle - 5")
for p in pos:
for m in self.reachy.head.motors:
m.goal_position = p
time.sleep(0.01)
print("###wiggle - 6")
self.set_head_compliance(False)
return wiggle_antennas
"""
def make_wiggle_antennas(self):
def wiggle_antennas():
self.set_head_compliance(False)
for m in self.reachy.head.motors:
m.goal_position = 45
time.sleep(0.5)
for m in self.reachy.head.motors:
m.goal_position = 0
time.sleep(0.5)
for m in self.reachy.head.motors:
m.goal_position = -45
time.sleep(0.5)
for m in self.reachy.head.motors:
m.goal_position = 0
time.sleep(0.5)
for m in self.reachy.head.motors:
m.goal_position = 45
time.sleep(0.5)
for m in self.reachy.head.motors:
m.goal_position = 0
time.sleep(0.5)
for m in self.reachy.head.motors:
m.goal_position = -45
time.sleep(0.5)
self.set_head_compliance(False)
return wiggle_antennas
def handle_wiggle_antennas(self, client=None, userdata=None, message=None):
self.action_queue.add(self.make_wiggle_antennas())
