forked from tangger/lerobot
fix teleop
This commit is contained in:
Sebastian-Contrari
@@ -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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user