update with newer reachy-mini api
Browse files- reachy_mini_app_example/main.py +13 -12
reachy_mini_app_example/main.py
CHANGED
@@ -11,17 +11,18 @@ class ExampleApp(ReachyMiniApp):
|
|
11 |
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
|
12 |
t0 = time.time()
|
13 |
while not stop_event.is_set():
|
14 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
15 |
|
16 |
-
|
17 |
|
18 |
-
|
19 |
-
head = np.eye(4)
|
20 |
-
head[:3, :3] = R.from_euler("xyz", [0, 0, yaw], degrees=False).as_matrix()
|
21 |
-
|
22 |
-
reachy_mini.set_position(head=head, antennas=np.array([target, -target]))
|
23 |
-
|
24 |
-
time.sleep(0.01)
|
25 |
-
# if more than one second since last ping, print ping
|
26 |
-
if t % 1 < 0.01:
|
27 |
-
print("Ping")
|
|
|
11 |
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
|
12 |
t0 = time.time()
|
13 |
while not stop_event.is_set():
|
14 |
+
pose = np.eye(4)
|
15 |
+
pose[:3, 3][2] = 0.005 * np.sin(2 * np.pi * 0.3 * time.time() + np.pi)
|
16 |
+
euler_rot = [
|
17 |
+
0,
|
18 |
+
0,
|
19 |
+
0.5 * np.sin(2 * np.pi * 0.3 * time.time() + np.pi),
|
20 |
+
]
|
21 |
+
rot_mat = R.from_euler("xyz", euler_rot, degrees=False).as_matrix()
|
22 |
+
pose[:3, :3] = rot_mat
|
23 |
+
pose[:3, 3][2] += 0.01 * np.sin(2 * np.pi * 0.5 * time.time())
|
24 |
+
antennas = np.array([1, 1]) * np.sin(2 * np.pi * 0.5 * time.time())
|
25 |
|
26 |
+
reachy_mini.set_target(head=pose, antennas=antennas)
|
27 |
|
28 |
+
time.sleep(0.02)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|