examples: pose_recorder (space=torque toggle, s=save) + pose_sender (send pose by id)

Author: RemiFabre

examples/pose_recorder.py

@@ -0,0 +1,119 @@
+"""Record head poses by hand.
+
+Workflow:
+    - SPACE toggles torque. Torque OFF -> move the head by hand; torque ON ->
+      it holds where you left it.
+    - S saves the current pose to a JSON file, printing its numeric ID.
+    - Q (or Ctrl-C) quits.
+
+Saved poses accumulate in the file (IDs 1, 2, 3, ...) so you can record a few,
+note which ID is which, then replay any of them with pose_sender.py.
+
+Note:
+    The daemon must be running. By default this connects to the Lite robot on
+    localhost; pass --robot wireless to reach reachy-mini.local.
+"""
+
+import argparse
+import json
+import sys
+import termios
+import tty
+from pathlib import Path
+
+import numpy as np
+
+from reachy_mini import ReachyMini
+
+
+def read_key() -> str:
+    """Read a single keypress from the terminal (raw mode)."""
+    fd = sys.stdin.fileno()
+    old = termios.tcgetattr(fd)
+    try:
+        tty.setraw(fd)
+        ch = sys.stdin.read(1)
+    finally:
+        termios.tcsetattr(fd, termios.TCSADRAIN, old)
+    return ch
+
+
+def connect(robot: str) -> ReachyMini:
+    """Connect to the Lite (localhost) or wireless (reachy-mini.local) robot."""
+    if robot == "wireless":
+        return ReachyMini(
+            media_backend="no_media",
+            connection_mode="network",
+            host="reachy-mini.local",
+        )
+    return ReachyMini(media_backend="no_media", connection_mode="localhost_only")
+
+
+def next_id(poses: list[dict]) -> int:
+    """Next sequential ID (1-based)."""
+    return max((p["id"] for p in poses), default=0) + 1
+
+
+def main() -> None:
+    """Run the interactive pose recorder."""
+    parser = argparse.ArgumentParser(description="Record head poses by hand.")
+    parser.add_argument(
+        "--robot",
+        choices=["lite", "wireless"],
+        default="lite",
+        help="Which robot to connect to (default: lite / localhost).",
+    )
+    parser.add_argument(
+        "--file",
+        default="recorded_poses.json",
+        help="JSON file to append poses to (default: recorded_poses.json).",
+    )
+    args = parser.parse_args()
+
+    path = Path(args.file).resolve()
+    poses = json.loads(path.read_text()) if path.exists() else []
+
+    print(f"Saving to {path}")
+    print("SPACE = toggle torque | S = save pose | Q = quit")
+
+    with connect(args.robot) as mini:
+        torque_on = True
+        mini.enable_motors()
+        print("Torque ON (holding). Press SPACE to release and move the head.")
+        try:
+            while True:
+                key = read_key()
+                if key in ("q", "\x03"):  # q or Ctrl-C
+                    break
+                if key == " ":
+                    torque_on = not torque_on
+                    if torque_on:
+                        mini.enable_motors()
+                        print("Torque ON  (holding current pose)")
+                    else:
+                        mini.disable_motors()
+                        print("Torque OFF (move the head by hand)")
+                elif key in ("s", "S"):
+                    head = np.array(mini.get_current_head_pose(), dtype=float)
+                    antennas = list(mini.get_present_antenna_joint_positions())
+                    pid = next_id(poses)
+                    poses.append(
+                        {
+                            "id": pid,
+                            "head": head.tolist(),
+                            "antennas": antennas,
+                        }
+                    )
+                    path.write_text(json.dumps(poses, indent=2))
+                    print(
+                        f"Saved pose {pid}: "
+                        f"pos={np.round(head[:3, 3], 4).tolist()} "
+                        f"antennas={np.round(antennas, 3).tolist()}"
+                    )
+        finally:
+            mini.enable_motors()
+            print("\nTorque ON. Bye.")
+
+
+if __name__ == "__main__":
+    main()

examples/pose_sender.py

@@ -0,0 +1,75 @@
+"""Send a recorded pose to a robot, then exit.
+
+Reads a pose (by ID) from the JSON file written by pose_recorder.py, enables
+torque, moves there, and quits. Everything else the robot does (handshakes,
+etc.) keeps working after.
+
+Note:
+    The daemon must be running. By default this connects to the Lite robot on
+    localhost; pass --robot wireless to reach reachy-mini.local (or run this
+    script directly on the wireless robot, where localhost is the wireless one).
+"""
+
+import argparse
+import json
+import sys
+from pathlib import Path
+
+import numpy as np
+
+from reachy_mini import ReachyMini
+
+
+def connect(robot: str) -> ReachyMini:
+    """Connect to the Lite (localhost) or wireless (reachy-mini.local) robot."""
+    if robot == "wireless":
+        return ReachyMini(
+            media_backend="no_media",
+            connection_mode="network",
+            host="reachy-mini.local",
+        )
+    return ReachyMini(media_backend="no_media", connection_mode="localhost_only")
+
+
+def main() -> None:
+    """Send one recorded pose to the robot and exit."""
+    parser = argparse.ArgumentParser(description="Send a recorded pose to a robot.")
+    parser.add_argument("id", type=int, help="Pose ID to send (see pose_recorder.py).")
+    parser.add_argument(
+        "--robot",
+        choices=["lite", "wireless"],
+        default="lite",
+        help="Which robot to send to (default: lite / localhost).",
+    )
+    parser.add_argument(
+        "--file",
+        default="recorded_poses.json",
+        help="JSON file of recorded poses (default: recorded_poses.json).",
+    )
+    parser.add_argument(
+        "--duration", type=float, default=1.0, help="Move duration in seconds."
+    )
+    parser.add_argument(
+        "--no-antennas", action="store_true", help="Send only the head pose."
+    )
+    args = parser.parse_args()
+
+    path = Path(args.file).resolve()
+    if not path.exists():
+        sys.exit(f"No pose file at {path}")
+    poses = {p["id"]: p for p in json.loads(path.read_text())}
+    if args.id not in poses:
+        sys.exit(f"No pose with ID {args.id} in {path}. Available: {sorted(poses)}")
+
+    pose = poses[args.id]
+    head = np.array(pose["head"], dtype=float)
+    antennas = None if args.no_antennas else pose.get("antennas")
+
+    with connect(args.robot) as mini:
+        mini.enable_motors()
+        mini.goto_target(head=head, antennas=antennas, duration=args.duration)
+    print(f"Sent pose {args.id} to {args.robot}.")
+
+
+if __name__ == "__main__":
+    main()