| 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: |
|
|
| |
| |
| mon_app = TestDance2() |
|
|
| stop_event = threading.Event() |
|
|
| try: |
| mon_app.run(robot, stop_event) |
| except KeyboardInterrupt: |
| print("\nArrêt du programme") |
| stop_event.set() |
