File size: 840 Bytes
b44cc91 |
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 |
import threading
import time
import numpy as np
from reachy_mini import ReachyMiniApp
from reachy_mini.reachy_mini import ReachyMini
from scipy.spatial.transform import Rotation as R
class ExampleApp(ReachyMiniApp):
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
t0 = time.time()
while not stop_event.is_set():
t = time.time() - t0
target = np.deg2rad(30) * np.sin(2 * np.pi * 0.5 * t)
yaw = target
head = np.eye(4)
head[:3, :3] = R.from_euler("xyz", [0, 0, yaw], degrees=False).as_matrix()
reachy_mini.set_position(head=head, antennas=np.array([target, -target]))
time.sleep(0.01)
# if more than one second since last ping, print ping
if t % 1 < 0.01:
print("Ping")
|