diff --git a/examples/hope_jr/teleop/left_arm_teleop.py b/examples/hope_jr/teleop/left_arm_teleop.py new file mode 100644 index 00000000..8577104d --- /dev/null +++ b/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() diff --git a/examples/hope_jr/teleop/left_hand_teleop.py b/examples/hope_jr/teleop/left_hand_teleop.py new file mode 100644 index 00000000..9e7eddeb --- /dev/null +++ b/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() diff --git a/examples/hope_jr/teleop/right_arm_teleop.py b/examples/hope_jr/teleop/right_arm_teleop.py new file mode 100644 index 00000000..143e241e --- /dev/null +++ b/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() diff --git a/examples/hope_jr/teleop/right_hand_teleop.py b/examples/hope_jr/teleop/right_hand_teleop.py new file mode 100644 index 00000000..d9626200 --- /dev/null +++ b/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() diff --git a/examples/hope_jr/vis/arm.py b/examples/hope_jr/vis/arm.py new file mode 100644 index 00000000..9a42943e --- /dev/null +++ b/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() diff --git a/examples/hope_jr/vis/exo.py b/examples/hope_jr/vis/exo.py new file mode 100644 index 00000000..8bff0135 --- /dev/null +++ b/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() diff --git a/examples/hope_jr/vis/glove.py b/examples/hope_jr/vis/glove.py new file mode 100644 index 00000000..a645ad14 --- /dev/null +++ b/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() diff --git a/examples/hope_jr/vis/hand.py b/examples/hope_jr/vis/hand.py new file mode 100644 index 00000000..84c72498 --- /dev/null +++ b/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()