Add control scripts

examples/hope_jr/teleop/left_arm_teleop.py

@@ -0,0 +1,53 @@
+import time
+import traceback
+
+from lerobot.common.robots.hope_jr import HopeJrArm, HopeJrArmConfig
+from lerobot.common.teleoperators.homonculus import HomonculusArm, HomonculusArmConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+
+def make_left_arm() -> tuple[HomonculusArm, HopeJrArm]:
+    left_exo_arm_cfg = HomonculusArmConfig("/dev/tty.usbmodem2101", id="left")
+    left_exo_arm = HomonculusArm(left_exo_arm_cfg)
+    left_arm_cfg = HopeJrArmConfig("/dev/tty.usbserial-140", id="left")
+    left_arm = HopeJrArm(left_arm_cfg)
+    return left_exo_arm, left_arm
+
+
+def main():
+    left_exo_arm, left_arm = make_left_arm()
+    display_len = max(len(key) for key in left_exo_arm.action_features)
+    left_exo_arm.connect()
+    left_arm.connect()
+
+    try:
+        while True:
+            start = time.perf_counter()
+
+            left_arm_action = left_exo_arm.get_action()
+            left_arm_action["shoulder_pitch.pos"] = -left_arm_action["shoulder_pitch.pos"]
+            left_arm_action["wrist_yaw.pos"] = -left_arm_action["wrist_yaw.pos"]
+            left_arm.send_action(left_arm_action)
+
+            time.sleep(0.01)
+            loop_s = time.perf_counter() - start
+
+            print("\n" + "-" * (display_len + 10))
+            print(f"{'NAME':<{display_len}} | {'ARM':>7}")
+            for joint, val in left_arm_action.items():
+                print(f"{joint:<{display_len}} | {val:>7.2f}")
+
+            print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+            move_cursor_up(len(left_arm_action) + 5)
+
+    except KeyboardInterrupt:
+        pass
+    except Exception:
+        traceback.print_exc()
+    finally:
+        left_exo_arm.disconnect()
+        left_arm.disconnect()
+
+
+if __name__ == "__main__":
+    main()

examples/hope_jr/teleop/left_hand_teleop.py

@@ -0,0 +1,51 @@
+import time
+import traceback
+
+from lerobot.common.robots.hope_jr import HopeJrHand, HopeJrHandConfig, homonculus_glove_to_hope_jr_hand
+from lerobot.common.teleoperators.homonculus import HomonculusGlove, HomonculusGloveConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+
+def make_left_hand() -> tuple[HomonculusGlove, HopeJrHand]:
+    left_glove_cfg = HomonculusGloveConfig("/dev/tty.usbmodem1101", id="left", side="left")
+    left_glove = HomonculusGlove(left_glove_cfg)
+    left_hand_cfg = HopeJrHandConfig("/dev/tty.usbmodem58760433641", id="left", side="left")
+    left_hand = HopeJrHand(left_hand_cfg)
+    return left_glove, left_hand
+
+
+def main():
+    left_glove, left_hand = make_left_hand()
+    left_glove.connect()
+    left_hand.connect()
+    display_len = max(len(key) for key in left_hand.action_features)
+
+    try:
+        while True:
+            start = time.perf_counter()
+
+            left_glove_action = left_glove.get_action()
+            left_hand_action = homonculus_glove_to_hope_jr_hand(left_glove_action)
+            left_hand.send_action(left_hand_action)
+
+            time.sleep(0.01)
+            loop_s = time.perf_counter() - start
+
+            print(f"\n{'NAME':<{display_len}} | {'HAND':>7}")
+            for joint, val in left_hand_action.items():
+                print(f"{joint:<{display_len}} | {val:>7.2f}")
+
+            print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+            move_cursor_up(len(left_hand_action) + 4)
+
+    except KeyboardInterrupt:
+        pass
+    except Exception:
+        traceback.print_exc()
+    finally:
+        left_glove.disconnect()
+        left_hand.disconnect()
+
+
+if __name__ == "__main__":
+    main()

examples/hope_jr/teleop/right_arm_teleop.py

@@ -0,0 +1,53 @@
+import time
+import traceback
+
+from lerobot.common.robots.hope_jr import HopeJrArm, HopeJrArmConfig
+from lerobot.common.teleoperators.homonculus import HomonculusArm, HomonculusArmConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+
+def make_right_arm() -> tuple[HomonculusArm, HopeJrArm]:
+    right_exo_arm_cfg = HomonculusArmConfig("/dev/tty.usbmodem2101", id="right")
+    right_exo_arm = HomonculusArm(right_exo_arm_cfg)
+    right_arm_cfg = HopeJrArmConfig("/dev/tty.usbserial-140", id="right")
+    right_arm = HopeJrArm(right_arm_cfg)
+    return right_exo_arm, right_arm
+
+
+def main():
+    right_exo_arm, right_arm = make_right_arm()
+    display_len = max(len(key) for key in right_exo_arm.action_features)
+    right_exo_arm.connect()
+    right_arm.connect()
+
+    try:
+        while True:
+            start = time.perf_counter()
+
+            right_arm_action = right_exo_arm.get_action()
+            right_arm_action["shoulder_pitch.pos"] = -right_arm_action["shoulder_pitch.pos"]
+            right_arm_action["wrist_yaw.pos"] = -right_arm_action["wrist_yaw.pos"]
+            right_arm.send_action(right_arm_action)
+
+            time.sleep(0.01)
+            loop_s = time.perf_counter() - start
+
+            print("\n" + "-" * (display_len + 10))
+            print(f"{'NAME':<{display_len}} | {'ARM':>7}")
+            for joint, val in right_arm_action.items():
+                print(f"{joint:<{display_len}} | {val:>7.2f}")
+
+            print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+            move_cursor_up(len(right_arm_action) + 5)
+
+    except KeyboardInterrupt:
+        pass
+    except Exception:
+        traceback.print_exc()
+    finally:
+        right_exo_arm.disconnect()
+        right_arm.disconnect()
+
+
+if __name__ == "__main__":
+    main()

examples/hope_jr/teleop/right_hand_teleop.py

@@ -0,0 +1,51 @@
+import time
+import traceback
+
+from lerobot.common.robots.hope_jr import HopeJrHand, HopeJrHandConfig, homonculus_glove_to_hope_jr_hand
+from lerobot.common.teleoperators.homonculus import HomonculusGlove, HomonculusGloveConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+
+def make_right_hand() -> tuple[HomonculusGlove, HopeJrHand]:
+    right_glove_cfg = HomonculusGloveConfig("/dev/tty.usbmodem2101", id="right", side="right")
+    right_glove = HomonculusGlove(right_glove_cfg)
+    right_hand_cfg = HopeJrHandConfig("/dev/tty.usbserial-140", id="right", side="right")
+    right_hand = HopeJrHand(right_hand_cfg)
+    return right_glove, right_hand
+
+
+def main():
+    right_glove, right_hand = make_right_hand()
+    right_glove.connect()
+    right_hand.connect()
+    display_len = max(len(key) for key in right_hand.action_features)
+
+    try:
+        while True:
+            start = time.perf_counter()
+
+            right_glove_action = right_glove.get_action()
+            right_hand_action = homonculus_glove_to_hope_jr_hand(right_glove_action)
+            right_hand.send_action(right_hand_action)
+
+            time.sleep(0.01)
+            loop_s = time.perf_counter() - start
+
+            print(f"\n{'NAME':<{display_len}} | {'HAND':>7}")
+            for joint, val in right_hand_action.items():
+                print(f"{joint:<{display_len}} | {val:>7.2f}")
+
+            print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+            move_cursor_up(len(right_hand_action) + 4)
+
+    except KeyboardInterrupt:
+        pass
+    except Exception:
+        traceback.print_exc()
+    finally:
+        right_glove.disconnect()
+        right_hand.disconnect()
+
+
+if __name__ == "__main__":
+    main()

examples/hope_jr/vis/arm.py

@@ -0,0 +1,36 @@
+import time
+import traceback
+
+from lerobot.common.robots.hope_jr import HopeJrArm, HopeJrArmConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+# cfg = HopeJrArmConfig("/dev/tty.usbserial-140", id="right")
+cfg = HopeJrArmConfig("/dev/tty.usbserial-140", id="left")
+arm = HopeJrArm(cfg)
+display_len = max(len(key) for key in arm.action_features)
+
+arm.connect()
+# arm.calibrate()
+arm.bus.disable_torque()
+try:
+    while True:
+        start = time.perf_counter()
+        raw_obs = arm.bus.sync_read("Present_Position", normalize=False)
+        norm_obs = {arm.bus.motors[name].id: val for name, val in raw_obs.items()}
+        norm_obs = arm.bus._normalize(norm_obs)
+        norm_obs = {arm.bus._id_to_name(id_): val for id_, val in norm_obs.items()}
+        loop_s = time.perf_counter() - start
+
+        print("\n----------------------------------")
+        print(f"{'NAME':<15} | {'RAW':>7} | {'NORM':>7}")
+        for motor in arm.bus.motors:
+            print(f"{motor:<15} | {raw_obs[motor]:>7} | {norm_obs[motor]:>7.2f}")
+        print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+        move_cursor_up(len(arm.bus.motors) + 5)
+
+except KeyboardInterrupt:
+    pass
+except Exception as e:
+    traceback.print_exc(e)
+finally:
+    arm.disconnect()

examples/hope_jr/vis/exo.py

@@ -0,0 +1,33 @@
+import time
+import traceback
+
+from lerobot.common.teleoperators.homonculus import HomonculusArm, HomonculusArmConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+# cfg = HomonculusArmConfig("/dev/tty.usbmodem2101", id="left")
+cfg = HomonculusArmConfig("/dev/tty.usbmodem2101", id="right")
+arm = HomonculusArm(cfg)
+display_len = max(len(key) for key in arm.action_features)
+
+arm.connect()
+# arm.calibrate()
+try:
+    while True:
+        start = time.perf_counter()
+        raw_action = arm._read(normalize=False)
+        norm_action = arm._normalize(raw_action)
+        loop_s = time.perf_counter() - start
+
+        print("\n" + "-" * (display_len + 10))
+        print(f"{'NAME':<{display_len}} | {'RAW':>7} | {'NORM':>7}")
+        for joint in arm.joints:
+            print(f"{joint:<{display_len}} | {raw_action[joint]:>7} | {norm_action[joint]:>7.2f}")
+        print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+        move_cursor_up(len(norm_action) + 5)
+
+except KeyboardInterrupt:
+    pass
+except Exception:
+    traceback.print_exc()
+finally:
+    arm.disconnect()

examples/hope_jr/vis/glove.py

@@ -0,0 +1,32 @@
+import time
+import traceback
+
+from lerobot.common.teleoperators.homonculus import HomonculusGlove, HomonculusGloveConfig
+from lerobot.common.utils.utils import move_cursor_up
+
+config = HomonculusGloveConfig("/dev/tty.usbmodem2101", side="right", id="right")
+glove = HomonculusGlove(config)
+glove.connect()
+
+display_len = max(len(key) for key in glove.action_features)
+glove.calibrate()
+try:
+    while True:
+        start = time.perf_counter()
+        raw_action = glove._read(normalize=False)
+        norm_action = glove._normalize(raw_action)
+        loop_s = time.perf_counter() - start
+
+        print("\n" + "-" * (display_len + 10))
+        print(f"{'NAME':<{display_len}} | {'RAW':>7} | {'NORM':>7}")
+        for joint in glove.joints:
+            print(f"{joint:<{display_len}} | {raw_action[joint]:>7} | {norm_action[joint]:>7.2f}")
+        print(f"\ntime: {loop_s * 1e3:.2f}ms ({1 / loop_s:.0f} Hz)")
+        move_cursor_up(len(norm_action) + 5)
+
+except KeyboardInterrupt:
+    pass
+except Exception:
+    traceback.print_exc()
+finally:
+    glove.disconnect()

examples/hope_jr/vis/hand.py

@@ -0,0 +1,8 @@
+from lerobot.common.robots.hope_jr import HopeJrHand, HopeJrHandConfig
+
+cfg = HopeJrHandConfig("/dev/tty.usbmodem58760433641", id="left", side="left")
+hand = HopeJrHand(cfg)
+
+hand.connect()
+hand.calibrate()
+hand.disconnect()