File size: 3,888 Bytes
13d375e | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 | import time
import threading
import numpy as np
from reachy_mini import ReachyMini, ReachyMiniApp
from reachy_mini.utils import create_head_pose
class TestDance1(ReachyMiniApp):
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
start_time = time.time()
while not stop_event.is_set():
t = time.time()
elapsed_time = t - start_time
compteur = int(elapsed_time / 2)
antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
current_roll = 0.0
current_pitch = 0.0
current_yaw = 0.0
current_x = 0.0
current_y = 0.0
current_z = 0.0
if compteur < 3:
current_roll = sinus_rot
elif compteur < 6:
current_pitch = sinus_rot
elif compteur < 9:
current_yaw = sinus_rot
elif compteur < 12:
current_x = sinus_trans
elif compteur < 15:
current_y = sinus_trans
elif compteur < 18:
current_z = sinus_trans
else:
start_time = time.time()
continue
head_pose = create_head_pose(
x=current_x,
y=current_y,
z=current_z,
roll=current_roll,
pitch=current_pitch,
yaw=current_yaw,
degrees=False,
mm=False,
)
reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])
time.sleep(0.01)
class TestDance2(ReachyMiniApp):
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
reachy_mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0)
start_time = time.time()
while not stop_event.is_set():
t = time.time()
elapsed_time = t - start_time
compteur = int(elapsed_time / 2)
antennas_offset = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * t))
sinus_rot = np.deg2rad(20 * np.sin(2 * np.pi * 0.5 * 4 * t))
sinus_trans = 0.03 * np.sin(2 * np.pi * 0.5 * t)
cosinus_trans = 0.03 * np.cos(2 * np.pi * 0.5 * t)
current_roll = 0.0
current_pitch = 0.0
current_yaw = 0.0
current_x = 0.0
current_y = 0.0
current_z = 0.0
if compteur < 3:
current_pitch = sinus_rot
current_y = sinus_trans
elif compteur < 6:
current_x = sinus_trans
current_y = cosinus_trans
else:
start_time = time.time()
continue
head_pose = create_head_pose(
x=current_x,
y=current_y,
z=current_z,
roll=current_roll,
pitch=current_pitch,
yaw=current_yaw,
degrees=False,
mm=False,
)
reachy_mini.set_target(head=head_pose, antennas=[antennas_offset, antennas_offset])
time.sleep(0.01)
if __name__ == "__main__":
print("Lancement manuel depuis le terminal...")
with ReachyMini(media_backend="no_media") as robot:
# 👉 Choisis ici ta danse
# mon_app = TestDance1()
mon_app = TestDance2()
stop_event = threading.Event()
try:
mon_app.run(robot, stop_event)
except KeyboardInterrupt:
print("\nArrêt du programme")
stop_event.set() |