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

fix teleop

Browse Source
This commit is contained in:
Sebastian-Contrari
2025-04-11 16:28:06 +02:00
parent 54b685053e
commit 0306e18640
29 changed files with 2670 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
examples/hopejr/exoskeleton/plottest7.py Normal file
View File

@@ -0,0 +1,64 @@
import serial
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from collections import deque
# Config
SERIAL_PORT = '/dev/ttyACM1' # Change as needed
BAUD_RATE = 115200
BUFFER_LEN = 200
# Sensor names in order
sensor_names = [
"wrist_roll",
"wrist_pitch",
"wrist_yaw",
"elbow_flex",
"shoulder_roll",
"shoulder_yaw",
"shoulder_pitch"
]
# Initialize buffers
sensor_data = {
name: deque([0]*BUFFER_LEN, maxlen=BUFFER_LEN)
for name in sensor_names
}
# Setup plot
fig, axes = plt.subplots(len(sensor_names), 1, figsize=(8, 12), sharex=True)
fig.tight_layout(pad=3.0)
lines = {}
for i, name in enumerate(sensor_names):
axes[i].set_title(name)
axes[i].set_xlim(0, BUFFER_LEN)
axes[i].set_ylim(0, 4096)
line, = axes[i].plot([], [], label=name)
axes[i].legend()
lines[name] = line
# Connect to serial
ser = serial.Serial(SERIAL_PORT, BAUD_RATE)
# Update function
def update(frame):
while ser.in_waiting:
line = ser.readline().decode().strip()
parts = line.split()
if len(parts) != 7:
continue
try:
values = list(map(int, parts))
except ValueError:
continue
for i, name in enumerate(sensor_names):
sensor_data[name].append(values[i])
for name in sensor_names:
x = range(len(sensor_data[name]))
lines[name].set_data(x, sensor_data[name])
return lines.values()
# Animate
ani = animation.FuncAnimation(fig, update, interval=50, blit=False)
plt.show()

126
examples/hopejr/exoskeleton/plottestmulti.py
View File

@@ -13,8 +13,9 @@ ser = serial.Serial(SERIAL_PORT, BAUD_RATE)
# How many data points to keep in the scrolling buffer
buffer_len = 200
# Create buffers for each sensor pair.
# We'll store them in a dict to keep things organized.
# -------------------------------------------------------------------
# 1) Sensor buffers for existing sensors + new wrist_pitch, wrist_yaw
# -------------------------------------------------------------------
sensor_buffers = {
'wrist_roll': {
'val1': deque([0]*buffer_len, maxlen=buffer_len),
@@ -35,39 +36,57 @@ sensor_buffers = {
'shoulder_roll': {
'val1': deque([0]*buffer_len, maxlen=buffer_len),
'val2': deque([0]*buffer_len, maxlen=buffer_len)
}
},
# --- New single-valued sensors ---
'wrist_pitch': {
'val1': deque([0]*buffer_len, maxlen=buffer_len) # Only one line
},
'wrist_yaw': {
'val1': deque([0]*buffer_len, maxlen=buffer_len) # Only one line
},
}
# Create a figure with 5 subplots (one for each sensor pair).
fig, axes = plt.subplots(5, 1, figsize=(8, 12), sharex=True)
# -------------------------------------------------------------------
# 2) Figure with 7 subplots (was 5). We keep the original 5 + 2 new.
# -------------------------------------------------------------------
fig, axes = plt.subplots(7, 1, figsize=(8, 14), sharex=True)
fig.tight_layout(pad=3.0)
# We'll store line references in a dict so we can update them in our update() function.
lines = {
'wrist_roll': [],
'elbow_pitch': [],
'shoulder_pitch': [],
'shoulder_yaw': [],
'shoulder_roll': []
}
# We'll store line references in a dict so we can update them in update().
lines = {}
# Set up each subplot
# -------------------------------------------------------------------
# 3) Define each subplot, including new ones at the end.
# -------------------------------------------------------------------
subplot_info = [
('wrist_roll', 'Wrist Roll (2,3)', axes[0]),
('elbow_pitch', 'Elbow Pitch (0,1)', axes[1]),
('shoulder_pitch', 'Shoulder Pitch (10,11)', axes[2]),
('shoulder_yaw', 'Shoulder Yaw (12,13)', axes[3]),
('shoulder_roll', 'Shoulder Roll (14,15)', axes[4])
('wrist_roll', 'Wrist Roll (2,3)', axes[0]),
('elbow_pitch', 'Elbow Pitch (0,1)', axes[1]),
('shoulder_pitch', 'Shoulder Pitch (10,11)', axes[2]),
('shoulder_yaw', 'Shoulder Yaw (12,13)', axes[3]),
('shoulder_roll', 'Shoulder Roll (14,15)', axes[4]),
('wrist_pitch', 'Wrist Pitch (0)', axes[5]), # new
('wrist_yaw', 'Wrist Yaw (1)', axes[6]), # new
]
# Set up each subplot
for (sensor_name, label, ax) in subplot_info:
ax.set_title(label)
ax.set_xlim(0, buffer_len)
ax.set_ylim(0, 4096)
line1, = ax.plot([], [], label=f"{sensor_name} - val1")
line2, = ax.plot([], [], label=f"{sensor_name} - val2")
ax.set_ylim(0, 4096) # adjust if needed
# For existing sensors, plot 2 lines (val1, val2)
# For the new single-line sensors, plot just 1 line
if sensor_name in ['wrist_pitch', 'wrist_yaw']:
# Single-valued
line, = ax.plot([], [], label=f"{sensor_name}")
lines[sensor_name] = line
else:
# Pair of values
line1, = ax.plot([], [], label=f"{sensor_name} - val1")
line2, = ax.plot([], [], label=f"{sensor_name} - val2")
lines[sensor_name] = [line1, line2]
ax.legend()
lines[sensor_name] = [line1, line2]
def update(frame):
# Read all available lines from the serial buffer
@@ -75,9 +94,8 @@ def update(frame):
raw_line = ser.readline().decode('utf-8').strip()
parts = raw_line.split()
# We expect at least 16 values if all sensors are present.
# (Because you mentioned indices 0..1, 2..3, 10..11, 12..13, 14..15)
if len(parts) < 16:
# We expect at least 16 values if all sensors are present
if len(parts) < 7:
continue
try:
@@ -86,40 +104,54 @@ def update(frame):
# If there's a parsing error, skip this line
continue
# Extract the relevant values and append to the correct buffer
sensor_buffers['elbow_pitch']['val1'].append(values[0])
sensor_buffers['elbow_pitch']['val2'].append(values[1])
# Original code: extract the relevant values and append to the correct buffer
sensor_buffers['elbow_pitch']['val1'].append(values[13])
sensor_buffers['elbow_pitch']['val2'].append(values[13])
sensor_buffers['wrist_roll']['val1'].append(values[2])
sensor_buffers['wrist_roll']['val1'].append(values[3])
sensor_buffers['wrist_roll']['val2'].append(values[3])
sensor_buffers['shoulder_pitch']['val1'].append(values[14])
sensor_buffers['shoulder_pitch']['val2'].append(values[15])
sensor_buffers['shoulder_pitch']['val2'].append(values[14])
sensor_buffers['shoulder_yaw']['val1'].append(values[12])
sensor_buffers['shoulder_yaw']['val2'].append(values[13])
sensor_buffers['shoulder_yaw']['val1'].append(values[8])
sensor_buffers['shoulder_yaw']['val2'].append(values[8])
sensor_buffers['shoulder_roll']['val1'].append(values[10])
sensor_buffers['shoulder_roll']['val2'].append(values[11])
sensor_buffers['shoulder_roll']['val2'].append(values[10])
# -------------------------------------------------------------------
# 4) New code: also read wrist_pitch (index 0) and wrist_yaw (index 1)
# -------------------------------------------------------------------
sensor_buffers['wrist_yaw']['val1'].append(values[0])
sensor_buffers['wrist_pitch']['val1'].append(values[1])
# Update each line's data in each subplot
all_lines = []
for (sensor_name, _, ax) in subplot_info:
# x-values are just the index range of the buffer
# x-values are just the index range of the buffer for val1
x_data = range(len(sensor_buffers[sensor_name]['val1']))
# First line
lines[sensor_name][0].set_data(
x_data,
sensor_buffers[sensor_name]['val1']
)
# Second line
lines[sensor_name][1].set_data(
x_data,
sensor_buffers[sensor_name]['val2']
)
all_lines.extend(lines[sensor_name])
# If this sensor has two lines
if isinstance(lines[sensor_name], list):
# First line
lines[sensor_name][0].set_data(
x_data,
sensor_buffers[sensor_name]['val1']
)
# Second line
lines[sensor_name][1].set_data(
x_data,
sensor_buffers[sensor_name]['val2']
)
all_lines.extend(lines[sensor_name])
else:
# Single line only (wrist_pitch, wrist_yaw)
lines[sensor_name].set_data(
x_data,
sensor_buffers[sensor_name]['val1']
)
all_lines.append(lines[sensor_name])
return all_lines

70
examples/hopejr/follower.py
View File

@@ -32,29 +32,29 @@ class HopeJuniorRobot:
motors = {
# Thumb
"thumb_basel_rotation": [1, "scs0009"],
"thumb_mcp": [2, "scs0009"],
"thumb_pip": [3, "scs0009"],
"thumb_dip": [4, "scs0009"],
"thumb_mcp": [3, "scs0009"],
"thumb_pip": [4, "scs0009"],
"thumb_dip": [13, "scs0009"],
# Index
"index_thumb_side": [5, "scs0009"],
"index_pinky_side": [6, "scs0009"],
"index_flexor": [7, "scs0009"],
"index_flexor": [16, "scs0009"],
# Middle
"middle_thumb_side": [8, "scs0009"],
"middle_pinky_side": [9, "scs0009"],
"middle_flexor": [10, "scs0009"],
"middle_flexor": [2, "scs0009"],
# Ring
"ring_thumb_side": [11, "scs0009"],
"ring_pinky_side": [12, "scs0009"],
"ring_flexor": [13, "scs0009"],
"ring_flexor": [7, "scs0009"],
# Pinky
"pinky_thumb_side": [14, "scs0009"],
"pinky_pinky_side": [15, "scs0009"],
"pinky_flexor": [16, "scs0009"],
"pinky_flexor": [10, "scs0009"],
},
protocol_version=1,#1
group_sync_read=False,
@@ -83,48 +83,48 @@ class HopeJuniorRobot:
start_pos = [
750, # thumb_basel_rotation
1000, # thumb_mcp
500, # thumb_pip
950, # thumb_dip
100, # thumb_mcp
700, # thumb_pip
100, # thumb_dip
150, # index_thumb_side
800, # index_thumb_side
950, # index_pinky_side
500, # index_flexor
0, # index_flexor
250, # middle_thumb_side
850, # middle_pinky_side
1000, # middle_flexor
0, # middle_flexor
850, # ring_thumb_side
900, # ring_pinky_side
600, # ring_flexor
0, # ring_flexor
00, # pinky_thumb_side
950, # pinky_pinky_side
150, # pinky_flexor
0, # pinky_flexor
]
end_pos = [
start_pos[0] - 550, # thumb_basel_rotation
start_pos[1] - 350, # thumb_mcp
start_pos[2] + 500, # thumb_pip
start_pos[3] - 550, # thumb_dip
start_pos[1] + 400, # thumb_mcp
start_pos[2] + 300, # thumb_pip
start_pos[3] + 200, # thumb_dip
start_pos[4] + 350, # index_thumb_side
start_pos[4] - 700, # index_thumb_side
start_pos[5] - 300, # index_pinky_side
start_pos[6] + 500, # index_flexor
start_pos[6] + 600, # index_flexor
start_pos[7] + 400, # middle_thumb_side
start_pos[7] + 700, # middle_thumb_side
start_pos[8] - 400, # middle_pinky_side
start_pos[9] - 650, # middle_flexor
start_pos[9] + 600, # middle_flexor
start_pos[10] - 400, # ring_thumb_side
start_pos[10] - 600, # ring_thumb_side
start_pos[11] - 400, # ring_pinky_side
start_pos[12] + 400, # ring_flexor
start_pos[12] + 600, # ring_flexor
start_pos[13] + 500, # pinky_thumb_side
start_pos[14] - 350, # pinky_pinky_side
start_pos[15] + 450, # pinky_flexor
start_pos[13] + 400, # pinky_thumb_side
start_pos[14] - 450, # pinky_pinky_side
start_pos[15] + 600, # pinky_flexor
]
@@ -148,23 +148,23 @@ class HopeJuniorRobot:
drive_mode = [0] * len(self.arm_bus.motor_names)
start_pos = [
1600, # shoulder_up
2450, # shoulder_forward
1700, # shoulder_roll
1150, # bend_elbow
1800, # shoulder_up
2800, # shoulder_forward
1800, # shoulder_roll
1200, # bend_elbow
700, # wrist_roll
1850, # wrist_yaw
1700, # wrist_pitch
]
end_pos = [
3100, # shoulder_up
2800, # shoulder_up
3150, # shoulder_forward
400, #shoulder_roll
2800, # bend_elbow
2600, # wrist_roll
2300, # bend_elbow
2300, # wrist_roll
2150, # wrist_yaw
2400, # wrist_pitch
2300, # wrist_pitch
]
calib_modes = [CalibrationMode.LINEAR.name] * len(self.arm_bus.motor_names)

52
examples/hopejr/read_arm.cs Normal file
View File

@@ -0,0 +1,52 @@
#include <Arduino.h>
// Define multiplexer input pins
#define S0 5
#define S1 6
#define S2 8
#define S3 7
#define SENSOR_INPUT 4
#define SENSOR_COUNT 16
int rawVals[SENSOR_COUNT];
void measureRawValues() {
for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
digitalWrite(S0, (i & 0b1) ^ 0b1);;
digitalWrite(S1, (i >> 1 & 0b1) ^ 0b1);;
digitalWrite(S2, (i >> 2 & 0b1) ^ 0b1);;
digitalWrite(S3, i >> 3 & 0b1);
delay(1);
rawVals[i] = analogRead(SENSOR_INPUT);
}
}
void printRawValues() {
for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
Serial.print(rawVals[i]);
if (i < SENSOR_COUNT - 1) Serial.print(" ");
}
Serial.println();
}
void setup() {
Serial.begin(115200);
pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
digitalWrite(S0, LOW);
digitalWrite(S1, LOW);
digitalWrite(S2, LOW);
digitalWrite(S3, LOW);
}
void loop() {
measureRawValues();
printRawValues();
delay(1);
}

52
examples/hopejr/read_glove.cs Normal file
View File

@@ -0,0 +1,52 @@
#include <Arduino.h>
// Define multiplexer input pins
#define S0 5
#define S1 6
#define S2 8
#define S3 7
#define SENSOR_INPUT 4
#define SENSOR_COUNT 16
int rawVals[SENSOR_COUNT];
void measureRawValues() {
for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
digitalWrite(S0, (i & 0b1) ^ 0b1);;
digitalWrite(S1, (i >> 1 & 0b1) ^ 0b1);;
digitalWrite(S2, (i >> 2 & 0b1) ^ 0b1);;
digitalWrite(S3, i >> 3 & 0b1);
delay(1);
rawVals[i] = analogRead(SENSOR_INPUT);
}
}
void printRawValues() {
for (uint8_t i = 0; i < SENSOR_COUNT; i++) {
Serial.print(rawVals[i]);
if (i < SENSOR_COUNT - 1) Serial.print(" ");
}
Serial.println();
}
void setup() {
Serial.begin(115200);
pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
digitalWrite(S0, LOW);
digitalWrite(S1, LOW);
digitalWrite(S2, LOW);
digitalWrite(S3, LOW);
}
void loop() {
measureRawValues();
printRawValues();
delay(1);
}

2
examples/hopejr/settings/config.yaml
View File

@@ -10,6 +10,6 @@ robot:
Protective_Torque: 1
Maximum_Acceleration: 100
Torque_Enable: 1
Acceleration: 100
Acceleration: 30
hand_bus:
Acceleration: 100

79
examples/hopejr/teleop.py
View File

@@ -23,45 +23,45 @@ def main(
robot.connect_hand()
#robot.connect_arm()
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')
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"]))
#robot.arm_bus.write("Goal_Position", robot.arm_calib_dict["start_pos"][0]*1 +robot.arm_calib_dict["end_pos"][0]*0, ["wrist_roll"])
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)
arm_calibration = robot.get_arm_calibration()
exoskeleton = HomonculusArm(serial_port="/dev/ttyACM2")
#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)
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/ttyACM2")
glove = HomonculusGlove(serial_port = "/dev/ttyACM0")
if calibrate_glove:
glove.run_calibration()
glove.run_calibration()
file_path = "examples/hopejr/settings/hand_calib.pkl"
with open(file_path, "rb") as f:
calib_dict = pickle.load(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)
robot.arm_bus.set_calibration(arm_calibration)
# Initialize Pygame
# pygame.init()
@@ -96,23 +96,30 @@ def main(
# 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)
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"]
#only wrist roll
#joint_names = ["shoulder_pitch"]
joint_values = exoskeleton.read(motor_names=joint_names)
# joint_values = joint_values.round().astype(int)
# joint_dict = {k: v for k, v in zip(joint_names, joint_values, strict=False)}
#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 = []
motor_names = []
motor_names += ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"]
#motor_names += ["shoulder_pitch"]
motor_values += [joint_dict[name] for name in motor_names]
#remove 50 from shoulder_roll
#motor_values += [joint_dict[name] 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)
motor_values = np.array(motor_values)
motor_values = np.clip(motor_values, 0, 100)
print(motor_names, motor_values)
if not freeze_arm:
robot.arm_bus.write("Goal_Position", motor_values, motor_names)
if not freeze_fingers:#include hand
hand_joint_names = []