File size: 981 Bytes
b44cc91 7b36dfe b44cc91 7b36dfe b44cc91 7b36dfe |
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 |
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():
pose = np.eye(4)
pose[:3, 3][2] = 0.005 * np.sin(2 * np.pi * 0.3 * time.time() + np.pi)
euler_rot = [
0,
0,
0.5 * np.sin(2 * np.pi * 0.3 * time.time() + np.pi),
]
rot_mat = R.from_euler("xyz", euler_rot, degrees=False).as_matrix()
pose[:3, :3] = rot_mat
pose[:3, 3][2] += 0.01 * np.sin(2 * np.pi * 0.5 * time.time())
antennas = np.array([1, 1]) * np.sin(2 * np.pi * 0.5 * time.time())
reachy_mini.set_target(head=pose, antennas=antennas)
time.sleep(0.02)
|