tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

hopejr teleop code

Browse Source
This commit is contained in:
Sebastian-Contrari
2025-04-02 16:19:15 +02:00
parent 91f549b2ce
commit f6e862d421
23 changed files with 2638 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

219
examples/hopejr/teleop.py Normal file
View File

@@ -0,0 +1,219 @@
from follower import HopeJuniorRobot
from leader import (
HomonculusArm,
HomonculusGlove,
EncoderReader
)
from visualizer import value_to_color
import time
import numpy as np
import pickle
import pygame
import typer
def main(
calibrate_glove: bool = typer.Option(False, "--calibrate-glove", help="Calibrate the glove"),
calibrate_exoskeleton: bool = typer.Option(False, "--calibrate-exoskeleton", help="Calibrate the exoskeleton"),
freeze_fingers: bool = typer.Option(False, "--freeze-fingers", help="Freeze the fingers"),
freeze_arm: bool = typer.Option(False, "--freeze-arm", help="Freeze the arm")):
show_loads: bool = typer.Option(False, "--show-loads", help="Show the loads in a GUI")
robot = HopeJuniorRobot()
robot.connect_hand()
#robot.connect_arm()
#read pos
print(robot.hand_bus.read("Present_Position"))
#print(robot.arm_bus.read("Present_Position", "shoulder_pitch"))
#print(robot.arm_bus.read("Present_Position",["shoulder_yaw","shoulder_roll","elbow_flex","wrist_roll","wrist_yaw","wrist_pitch"]))
#for i in range(10):
# time.sleep(0.1)
# robot.apply_arm_config('examples/hopejr/settings/config.yaml')
# #calibrate arm
#arm_calibration = robot.get_arm_calibration()
#exoskeleton = HomonculusArm(serial_port="/dev/ttyACM2")
#robot.arm_bus.write("Goal_Position", robot.arm_calib_dict["start_pos"][0]*0.7 + robot.arm_calib_dict["end_pos"][0]*0.3, ["shoulder_pitch"])
#if calibrate_exoskeleton:
# exoskeleton.run_calibration(robot)
#file_path = "examples/hopejr/settings/arm_calib.pkl"
#with open(file_path, "rb") as f:
# calib_dict = pickle.load(f)
#print("Loaded dictionary:", calib_dict)
#exoskeleton.set_calibration(calib_dict)
#calibrate hand
hand_calibration = robot.get_hand_calibration()
glove = HomonculusGlove(serial_port = "/dev/ttyACM1")
if calibrate_glove:
glove.run_calibration()
file_path = "examples/hopejr/settings/hand_calib.pkl"
with open(file_path, "rb") as f:
calib_dict = pickle.load(f)
print("Loaded dictionary:", calib_dict)
glove.set_calibration(calib_dict)
robot.hand_bus.set_calibration(hand_calibration)
#robot.arm_bus.set_calibration(arm_calibration)
# Initialize Pygame
# pygame.init()
# # Set up the display
# screen = pygame.display.set_mode((800, 600))
# pygame.display.set_caption("Robot Hand Visualization")
# # Create hand structure with 16 squares and initial values
# hand_components = []
# # Add thumb (4 squares in diamond shape)
# thumb_positions = [
# (150, 300), (125, 350),
# (175, 350), (150, 400)
# ]
# for pos in thumb_positions:
# hand_components.append({"pos": pos, "value": 0})
# # Add fingers (4 fingers with 3 squares each in vertical lines)
# finger_positions = [
# (200, 100), # Index
# (250, 100), # Middle
# (300, 100), # Ring
# (350, 100) # Pinky
# ]
# for x, y in finger_positions:
# for i in range(3):
# hand_components.append({"pos": (x, y + i * 50), "value": 0})
for i in range(1000000000000000):
# robot.apply_arm_config('examples/hopejr/settings//config.yaml')
# robot.arm_bus.write("Acceleration", 50, "shoulder_yaw")
# joint_names = ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"]
# joint_values = exoskeleton.read_running_average(motor_names=joint_names, linearize=True)
# joint_values = joint_values.round().astype(int)
# joint_dict = {k: v for k, v in zip(joint_names, joint_values, strict=False)}
# motor_values = []
# motor_names = []
# motor_names += ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"]
# motor_values += [joint_dict[name]-30 for name in motor_names]
# motor_values = np.array(motor_values)
# motor_values = np.clip(motor_values, 0, 100)
# if not freeze_arm:
# robot.arm_bus.write("Goal_Position", motor_values, motor_names)
if not freeze_fingers:#include hand
hand_joint_names = []
hand_joint_names += ["thumb_3", "thumb_2", "thumb_1", "thumb_0"]#, "thumb_3"]
hand_joint_names += ["index_0", "index_1", "index_2"]
hand_joint_names += ["middle_0", "middle_1", "middle_2"]
hand_joint_names += ["ring_0", "ring_1", "ring_2"]
hand_joint_names += ["pinky_0", "pinky_1", "pinky_2"]
hand_joint_values = glove.read(hand_joint_names)
hand_joint_values = hand_joint_values.round( ).astype(int)
hand_joint_dict = {k: v for k, v in zip(hand_joint_names, hand_joint_values, strict=False)}
hand_motor_values = []
hand_motor_names = []
# Thumb
hand_motor_names += ["thumb_basel_rotation", "thumb_mcp", "thumb_pip", "thumb_dip"]#, "thumb_MCP"]
hand_motor_values += [
hand_joint_dict["thumb_3"],
hand_joint_dict["thumb_2"],
hand_joint_dict["thumb_1"],
hand_joint_dict["thumb_0"]
]
# # Index finger
index_splay = 0.1
hand_motor_names += ["index_flexor", "index_pinky_side", "index_thumb_side"]
hand_motor_values += [
hand_joint_dict["index_2"],
(100 - hand_joint_dict["index_0"]) * index_splay + hand_joint_dict["index_1"] * (1 - index_splay),
hand_joint_dict["index_0"] * index_splay + hand_joint_dict["index_1"] * (1 - index_splay),
]
# Middle finger
middle_splay = 0.1
hand_motor_names += ["middle_flexor", "middle_pinky_side", "middle_thumb_side"]
hand_motor_values += [
hand_joint_dict["middle_2"],
hand_joint_dict["middle_0"] * middle_splay + hand_joint_dict["middle_1"] * (1 - middle_splay),
(100 - hand_joint_dict["middle_0"]) * middle_splay + hand_joint_dict["middle_1"] * (1 - middle_splay),
]
# # Ring finger
ring_splay = 0.1
hand_motor_names += ["ring_flexor", "ring_pinky_side", "ring_thumb_side"]
hand_motor_values += [
hand_joint_dict["ring_2"],
(100 - hand_joint_dict["ring_0"]) * ring_splay + hand_joint_dict["ring_1"] * (1 - ring_splay),
hand_joint_dict["ring_0"] * ring_splay + hand_joint_dict["ring_1"] * (1 - ring_splay),
]
# # Pinky finger
pinky_splay = -.1
hand_motor_names += ["pinky_flexor", "pinky_pinky_side", "pinky_thumb_side"]
hand_motor_values += [
hand_joint_dict["pinky_2"],
hand_joint_dict["pinky_0"] * pinky_splay + hand_joint_dict["pinky_1"] * (1 - pinky_splay),
(100 - hand_joint_dict["pinky_0"]) * pinky_splay + hand_joint_dict["pinky_1"] * (1 - pinky_splay),
]
hand_motor_values = np.array(hand_motor_values)
hand_motor_values = np.clip(hand_motor_values, 0, 100)
robot.hand_bus.write("Acceleration", 255, hand_motor_names)
robot.hand_bus.write("Goal_Position", hand_motor_values, hand_motor_names)
# if i%20==0 and i > 100:
# try:
# loads = robot.hand_bus.read("Present_Load")
# for i, comp in enumerate(hand_components):
# # Wave oscillates between 0 and 2024:
# # Center (1012) +/- 1012 * sin(...)
# comp["value"] = loads[i]
# except:
# pass
time.sleep(0.01)
# for event in pygame.event.get():
# if event.type == pygame.QUIT:
# robot.hand_bus.disconnect()
# robot.arm_bus.disconnect()
# exit()
# # Check for user pressing 'q' to quit
# if event.type == pygame.KEYDOWN:
# if event.key == pygame.K_q:
# robot.hand_bus.disconnect()
# robot.arm_bus.disconnect()
# exit()
# # Draw background
# screen.fill((0, 0, 0)) # Black background
# # Draw hand components
# for comp in hand_components:
# x, y = comp["pos"]
# color = value_to_color(comp["value"])
# pygame.draw.rect(screen, color, (x, y, 30, 30))
# pygame.display.flip()
if __name__ == "__main__":
typer.run(main)