wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix teleop

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
examples.zip Normal file
View File

Binary file not shown.

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

77
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"])
arm_calibration = robot.get_arm_calibration()
exoskeleton = HomonculusArm(serial_port="/dev/ttyACM2")
#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)
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 = []

97
examples/test_torque/faulty_servo.py Normal file
View File

@ -0,0 +1,97 @@
#faulty servo
Model = [777]
ID = [7]
Baud_Rate = [0]
Return_Delay = [0]
Response_Status_Level = [1]
Min_Angle_Limit = [1140]
Max_Angle_Limit = [2750]
Max_Temperature_Limit = [70]
Max_Voltage_Limit = [140]
Min_Voltage_Limit = [40]
Max_Torque_Limit = [1000]
Phase = [12]
Unloading_Condition = [44]
LED_Alarm_Condition = [47]
P_Coefficient = [32]
D_Coefficient = [32]
I_Coefficient = [0]
Minimum_Startup_Force = [16]
CW_Dead_Zone = [1]
CCW_Dead_Zone = [1]
Protection_Current = [310]
Angular_Resolution = [1]
Offset = [1047]
Mode = [0]
Protective_Torque = [20]
Protection_Time = [200]
Overload_Torque = [80]
Speed_closed_loop_P_proportional_coefficient = [10]
Over_Current_Protection_Time = [200]
Velocity_closed_loop_I_integral_coefficient = [200]
Torque_Enable = [1]
Acceleration = [20]
Goal_Position = [0]
Goal_Time = [0]
Goal_Speed = [0]
Torque_Limit = [1000]
Lock = [1]
Present_Position = [1494]
Present_Speed = [0]
Present_Load = [0]
Present_Voltage = [123]
Present_Temperature = [24]
Status = [0]
Moving = [0]
Present_Current = [0]
Maximum_Acceleration = [306]
#all servos of hopejr
Model = [2825 777 777 2825 777 777 777]
ID = [1 2 3 4 5 6 7]
Baud_Rate = [0 0 0 0 0 0 0]
Return_Delay = [0 0 0 0 0 0 0]
Response_Status_Level = [1 1 1 1 1 1 1]
Min_Angle_Limit = [ 650 1300 1300 1200 600 1725 0]
Max_Angle_Limit = [2600 2050 2800 2500 4096 2250 4095]
Max_Temperature_Limit = [80 70 70 80 70 70 70]
Max_Voltage_Limit = [160 140 140 160 140 140 80]
Min_Voltage_Limit = [60 40 40 60 40 40 40]
Max_Torque_Limit = [1000 1000 1000 1000 1000 1000 1000]
Phase = [12 12 12 12 12 12 12]
Unloading_Condition = [45 44 44 45 44 44 44]
LED_Alarm_Condition = [45 47 47 45 47 47 47]
P_Coefficient = [32 32 32 32 32 32 32]
D_Coefficient = [32 32 32 32 32 32 32]
I_Coefficient = [0 0 0 0 0 0 0]
Minimum_Startup_Force = [15 16 16 12 16 16 16]
CW_Dead_Zone = [0 1 1 0 1 1 1]
CCW_Dead_Zone = [0 1 1 0 1 1 1]
Protection_Current = [310 310 310 310 310 310 500]
Angular_Resolution = [1 1 1 1 1 1 1]
Offset = [ 0 1047 1024 1047 1024 1024 0]
Mode = [0 0 0 0 0 0 0]
Protective_Torque = [20 20 20 20 20 20 20]
Protection_Time = [200 200 200 200 200 200 200]
Overload_Torque = [80 80 80 80 80 80 80]
Speed_closed_loop_P_proportional_coefficient = [10 10 10 10 10 10 10]
Over_Current_Protection_Time = [250 200 200 250 200 200 200]
Velocity_closed_loop_I_integral_coefficient = [200 200 200 200 200 200 200]
Torque_Enable = [1 1 1 1 1 1 1]
Acceleration = [20 20 20 20 20 20 20]
Goal_Position = [1909 1977 1820 1000 707 1941 1036]
Goal_Time = [0 0 0 0 0 0 0]
Goal_Speed = [0 0 0 0 0 0 0]
Torque_Limit = [1000 1000 1000 200 1000 1000 1000]
Lock = [1 1 1 1 1 1 1]
Present_Position = [1909 1982 1821 1200 710 1941 1036]
Present_Speed = [0 0 0 0 0 0 0]
Present_Load = [ 0 48 0 0 32 0 0]
Present_Voltage = [122 123 122 123 122 122 122]
Present_Temperature = [23 28 28 26 29 28 28]
Status = [0 0 0 0 0 0 1]
Moving = [0 0 0 0 0 0 0]
Present_Current = [0 1 0 1 1 0 1]
Maximum_Acceleration = [1530 306 306 1530 306 306 254]

192
examples/test_torque/hopejr.py Normal file
View File

@ -0,0 +1,192 @@
import threading
import time
from typing import Callable
import cv2
import numpy as np
import serial
from lerobot.common.robot_devices.motors.feetech import (
CalibrationMode,
FeetechMotorsBus,
)
LOWER_BOUND_LINEAR = -100
UPPER_BOUND_LINEAR = 200
ESCAPE_KEY_ID = 27
class HopeJuniorRobot:
def __init__(self):
self.arm_bus = FeetechMotorsBus(
port="/dev/ttyACM1",
motors={
# "motor1": (2, "sts3250"),
# "motor2": (1, "scs0009"),
#"shoulder_pitch": [1, "sts3250"],
#"shoulder_yaw": [2, "sts3215"], # TODO: sts3250
#"shoulder_roll": [3, "sts3215"], # TODO: sts3250
#"elbow_flex": [4, "sts3250"],
#"wrist_roll": [5, "sts3215"],
#"wrist_yaw": [6, "sts3215"],
"wrist_pitch": [7, "sts3215"],
},
protocol_version=0,
)
self.hand_bus = FeetechMotorsBus(
port="/dev/ttyACM1",
motors={
"thumb_basel_rotation": [30, "scs0009"],
"thumb_flexor": [27, "scs0009"],
"thumb_pinky_side": [26, "scs0009"],
"thumb_thumb_side": [28, "scs0009"],
"index_flexor": [25, "scs0009"],
"index_pinky_side": [31, "scs0009"],
"index_thumb_side": [32, "scs0009"],
"middle_flexor": [24, "scs0009"],
"middle_pinky_side": [33, "scs0009"],
"middle_thumb_side": [34, "scs0009"],
"ring_flexor": [21, "scs0009"],
"ring_pinky_side": [36, "scs0009"],
"ring_thumb_side": [35, "scs0009"],
"pinky_flexor": [23, "scs0009"],
"pinky_pinky_side": [38, "scs0009"],
"pinky_thumb_side": [37, "scs0009"],
},
protocol_version=1,
group_sync_read=False,
)
def get_hand_calibration(self):
"""
Returns a dictionary containing calibration settings for each motor
on the hand bus.
"""
homing_offset = [0] * len(self.hand_bus.motor_names)
drive_mode = [0] * len(self.hand_bus.motor_names)
start_pos = [
500, 900, 0, 1000, 100, 250, 750, 100, 400, 150, 100, 120, 980, 100, 950, 750,
]
end_pos = [
start_pos[0] - 400, # 500 - 400 = 100
start_pos[1] - 300, # 900 - 300 = 600
start_pos[2] + 550, # 0 + 550 = 550
start_pos[3] - 550, # 1000 - 550 = 450
start_pos[4] + 900, # 100 + 900 = 1000
start_pos[5] + 500, # 250 + 500 = 750
start_pos[6] - 500, # 750 - 500 = 250
start_pos[7] + 900, # 100 + 900 = 1000
start_pos[8] + 700, # 400 + 700 = 1100
start_pos[9] + 700, # 150 + 700 = 850
start_pos[10] + 900, # 100 + 900 = 1000
start_pos[11] + 700, # 120 + 700 = 820
start_pos[12] - 700, # 980 - 700 = 280
start_pos[13] + 900, # 100 + 900 = 1000
start_pos[14] - 700, # 950 - 700 = 250
start_pos[15] - 700, # 750 - 700 = 50
]
calib_modes = [CalibrationMode.LINEAR.name] * len(self.hand_bus.motor_names)
calib_dict = {
"homing_offset": homing_offset,
"drive_mode": drive_mode,
"start_pos": start_pos,
"end_pos": end_pos,
"calib_mode": calib_modes,
"motor_names": self.hand_bus.motor_names,
}
return calib_dict
def get_arm_calibration(self):
"""
Returns a dictionary containing calibration settings for each motor
on the arm bus.
"""
homing_offset = [0] * len(self.arm_bus.motor_names)
drive_mode = [0] * len(self.arm_bus.motor_names)
# Example placeholders
start_pos = [
600, # shoulder_up
1500, # shoulder_forward
1300, # shoulder_yaw
1000, # bend_elbow
1600, # wrist_roll
1700, # wrist_yaw
600, # wrist_pitch
]
end_pos = [
2300, # shoulder_up
2300, # shoulder_forward
2800, # shoulder_yaw
2500, # bend_elbow
2800, # wrist_roll
2200, # wrist_yaw
1700, # wrist_pitch
]
calib_modes = [CalibrationMode.LINEAR.name] * len(self.arm_bus.motor_names)
calib_dict = {
"homing_offset": homing_offset,
"drive_mode": drive_mode,
"start_pos": start_pos,
"end_pos": end_pos,
"calib_mode": calib_modes,
"motor_names": self.arm_bus.motor_names,
}
return calib_dict
def connect(self):
"""Connect to the Feetech buses."""
self.arm_bus.connect()
# self.hand_bus.connect()
def capture_and_display_processed_frames(
frame_processor: Callable[[np.ndarray], np.ndarray],
window_display_name: str,
cap_device: int = 0,
) -> None:
"""
Capture frames from the given input camera device, run them through
the frame processor, and display the outputs in a window with the given name.
User should press Esc to exit.
Inputs:
frame_processor: Callable[[np.ndarray], np.ndarray]
Processes frames.
Input and output are numpy arrays of shape (H W C) with BGR channel layout and dtype uint8 / byte.
window_display_name: str
Name of the window used to display frames.
cap_device: int
Identifier for the camera to use to capture frames.
"""
cv2.namedWindow(window_display_name)
capture = cv2.VideoCapture(cap_device)
if not capture.isOpened():
raise ValueError("Unable to open video capture.")
frame_count = 0
has_frame, frame = capture.read()
while has_frame:
frame_count = frame_count + 1
# Mirror frame horizontally and flip color for demonstration
frame = np.ascontiguousarray(frame[:, ::-1, ::-1])
# process & show frame
processed_frame = frame_processor(frame)
cv2.imshow(window_display_name, processed_frame[:, :, ::-1])
has_frame, frame = capture.read()
key = cv2.waitKey(1)
if key == ESCAPE_KEY_ID:
break
capture.release()

BIN
examples/test_torque/log_50ms_shoulder_pitch.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 56 KiB

BIN
examples/test_torque/log_50ms_shoulder_pitch_CURRENT.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 54 KiB

BIN
examples/test_torque/log_50ms_shoulder_pitch_LOAD.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 56 KiB

44
examples/test_torque/log_and_plot_feetech.py Normal file
View File

@ -0,0 +1,44 @@
import matplotlib.pyplot as plt
import time
from typing import List, Tuple
def log_and_plot_params(bus, params_to_log: list, servo_names: list,
test_id="servo_log", interval=0.1, duration=5, save_plot=True) -> Tuple[dict, List[float]]:
"""
Logs specific servo parameters for a given duration and generates a plot.
"""
servo_data = {servo_name: {param: [] for param in params_to_log} for servo_name in servo_names}
timestamps = []
start_time = time.time()
while time.time() - start_time < duration:
timestamp = time.time() - start_time
timestamps.append(timestamp)
for param in params_to_log:
values = bus.read(param, servo_names)
for servo_name, value in zip(servo_names, values):
servo_data[servo_name][param].append(value)
time.sleep(interval)
if save_plot:
for servo_name, data in servo_data.items():
plt.figure(figsize=(10, 6))
for param in params_to_log:
if all(v is not None for v in data[param]):
plt.plot(timestamps, data[param], label=param)
plt.xlabel("Time (s)")
plt.ylabel("Parameter Values")
plt.title(f"Parameter Trends for Servo: {servo_name}")
plt.legend()
plt.grid(True)
plt.tight_layout()
plot_filename = f"{test_id}_{servo_name}.png"
plt.savefig(plot_filename)
print(f"Plot saved as {plot_filename}")
return servo_data, timestamps

BIN
examples/test_torque/plots/log_200ms_shoulder_pitch_current.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 65 KiB

BIN
examples/test_torque/plots/log_50ms_shoulder_pitch.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 60 KiB

BIN
examples/test_torque/plots/log_50ms_shoulder_pitch_current.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 54 KiB

68
examples/test_torque/print_all_params.py Normal file
View File

@ -0,0 +1,68 @@
STS_SERIES_CONTROL_TABLE = {
"Model": (3, 2),
"ID": (5, 1),
"Baud_Rate": (6, 1),
"Return_Delay": (7, 1),
"Response_Status_Level": (8, 1),
"Min_Angle_Limit": (9, 2),
"Max_Angle_Limit": (11, 2),
"Max_Temperature_Limit": (13, 1),
"Max_Voltage_Limit": (14, 1),
"Min_Voltage_Limit": (15, 1),
"Max_Torque_Limit": (16, 2),
"Phase": (18, 1),
"Unloading_Condition": (19, 1),
"LED_Alarm_Condition": (20, 1),
"P_Coefficient": (21, 1),
"D_Coefficient": (22, 1),
"I_Coefficient": (23, 1),
"Minimum_Startup_Force": (24, 2),
"CW_Dead_Zone": (26, 1),
"CCW_Dead_Zone": (27, 1),
"Protection_Current": (28, 2),
"Angular_Resolution": (30, 1),
"Offset": (31, 2),
"Mode": (33, 1),
"Protective_Torque": (34, 1),
"Protection_Time": (35, 1),
"Overload_Torque": (36, 1),
"Speed_closed_loop_P_proportional_coefficient": (37, 1),
"Over_Current_Protection_Time": (38, 1),
"Velocity_closed_loop_I_integral_coefficient": (39, 1),
"Torque_Enable": (40, 1),
"Acceleration": (41, 1),
"Goal_Position": (42, 2),
"Goal_Time": (44, 2),
"Goal_Speed": (46, 2),
"Torque_Limit": (48, 2),
"Lock": (55, 1),
"Present_Position": (56, 2),
"Present_Speed": (58, 2),
"Present_Load": (60, 2),
"Present_Voltage": (62, 1),
"Present_Temperature": (63, 1),
"Status": (65, 1),
"Moving": (66, 1),
"Present_Current": (69, 2),
# Not in the Memory Table
"Maximum_Acceleration": (85, 2),
}
import time
# Assuming STS_SERIES_CONTROL_TABLE is defined globally
def print_all_params(robot):
"""
Reads all parameters from the STS_SERIES_CONTROL_TABLE and prints their values.
"""
for param in STS_SERIES_CONTROL_TABLE.keys():
try:
val = robot.arm_bus.read(param)
print(f"{param} = {val}")
except Exception as e:
print(f"{param} read failed: {e}")
# Example usage:
print_all_params(robot)

26
examples/test_torque/read_encoder.cs Normal file
View File

@ -0,0 +1,26 @@
#include <DFRobot_VisualRotaryEncoder.h>
DFRobot_VisualRotaryEncoder_I2C sensor(0x54, &Wire);
void setup()
{
Serial.begin(115200);
// Attempt to initialize the sensor
while (NO_ERR != sensor.begin()) {
// Failed? Just wait a bit and try again
delay(3000);
}
}
void loop()
{
// Read the encoder value
uint16_t encoderValue = sensor.getEncoderValue();
// Print it followed by a newline
Serial.println(encoderValue);
// Delay 10ms between readings
delay(10);
}

544
examples/test_torque/test_torque.ipynb Normal file
View File

@ -0,0 +1,544 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"what are the actual interest values on the hopejr? make like a map"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"can change these dynamically so if the arm is moving down we can relax it instead of tensing it? so for example decreasing torque if the target position is lower than the actual position, for example. "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"arm_calibration = robot.get_arm_calibration()\n",
"robot.arm_bus.write(\"Goal_Position\", arm_calibration[\"start_pos\"])"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Present Position: [1494]\n",
"Acceleration Read: [20]\n"
]
}
],
"source": [
"import time\n",
"from hopejr import HopeJuniorRobot\n",
"\n",
"\n",
"robot = HopeJuniorRobot()\n",
"robot.connect()\n",
"\n",
"# Example read of the current position\n",
"print(\"Present Position:\", robot.arm_bus.read(\"Present_Position\"))\n",
"\n",
"# Enable torque and set acceleration\n",
"robot.arm_bus.write(\"Torque_Enable\", 1)\n",
"robot.arm_bus.write(\"Acceleration\", 20)\n",
"print(\"Acceleration Read:\", robot.arm_bus.read(\"Acceleration\"))\n"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"Torque_Limit\", 100,[\"elbow_flex\"])\n",
"robot.arm_bus.write(\"Protective_Torque\", 0, [\"elbow_flex\"])\n",
"robot.arm_bus.write(\"Acceleration\", 20)\n",
"robot.arm_bus.write(\"Goal_Position\", [2000], [\"elbow_flex\"])"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([1000, 1000, 1000, 1000, 1000, 1000, 1000])"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.arm_bus.read(\"Max_Torque_Limit\")"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"Goal_Position\", [1909, 1977, 1820, 1000, 707, 1941, 1036]) #"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"Max_Voltage_Limit\", [160, 140, 140, 160, 140, 140, 140]) #so its not torque limit nor max torque limit, , no protective torque or overload torque\n",
"#it's 1) max voltage limit, min-max angle limits are arbitrairly set for all the motors, max temp is only set for the shoulder\n",
"#max acceleration is also set, we could lower that in the elbow to make it less responsive to commands basically\n",
"#so we limit speed and temperature, maybe we should limit torque thouhg, minimum startup force is also important. protection current as well\n",
"#changed that to 310.\n",
"#\"Max_Voltage_Limit\" also needs to be changed, different from torque_limit"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Model = [777]\n",
"ID = [7]\n",
"Baud_Rate = [0]\n",
"Return_Delay = [0]\n",
"Response_Status_Level = [1]\n",
"Min_Angle_Limit = [1140]\n",
"Max_Angle_Limit = [2750]\n",
"Max_Temperature_Limit = [70]\n",
"Max_Voltage_Limit = [140]\n",
"Min_Voltage_Limit = [40]\n",
"Max_Torque_Limit = [1000]\n",
"Phase = [12]\n",
"Unloading_Condition = [44]\n",
"LED_Alarm_Condition = [47]\n",
"P_Coefficient = [32]\n",
"D_Coefficient = [32]\n",
"I_Coefficient = [0]\n",
"Minimum_Startup_Force = [16]\n",
"CW_Dead_Zone = [1]\n",
"CCW_Dead_Zone = [1]\n",
"Protection_Current = [310]\n",
"Angular_Resolution = [1]\n",
"Offset = [1047]\n",
"Mode = [0]\n",
"Protective_Torque = [20]\n",
"Protection_Time = [200]\n",
"Overload_Torque = [80]\n",
"Speed_closed_loop_P_proportional_coefficient = [10]\n",
"Over_Current_Protection_Time = [200]\n",
"Velocity_closed_loop_I_integral_coefficient = [200]\n",
"Torque_Enable = [1]\n",
"Acceleration = [20]\n",
"Goal_Position = [0]\n",
"Goal_Time = [0]\n",
"Goal_Speed = [0]\n",
"Torque_Limit = [1000]\n",
"Lock = [1]\n",
"Present_Position = [1494]\n",
"Present_Speed = [0]\n",
"Present_Load = [0]\n",
"Present_Voltage = [123]\n",
"Present_Temperature = [24]\n",
"Status = [0]\n",
"Moving = [0]\n",
"Present_Current = [0]\n",
"Maximum_Acceleration = [306]\n"
]
}
],
"source": [
"STS_SERIES_CONTROL_TABLE = {\n",
" \"Model\": (3, 2),\n",
" \"ID\": (5, 1),\n",
" \"Baud_Rate\": (6, 1),\n",
" \"Return_Delay\": (7, 1),\n",
" \"Response_Status_Level\": (8, 1),\n",
" \"Min_Angle_Limit\": (9, 2),\n",
" \"Max_Angle_Limit\": (11, 2),\n",
" \"Max_Temperature_Limit\": (13, 1),\n",
" \"Max_Voltage_Limit\": (14, 1),\n",
" \"Min_Voltage_Limit\": (15, 1),\n",
" \"Max_Torque_Limit\": (16, 2),\n",
" \"Phase\": (18, 1),\n",
" \"Unloading_Condition\": (19, 1),\n",
" \"LED_Alarm_Condition\": (20, 1),\n",
" \"P_Coefficient\": (21, 1),\n",
" \"D_Coefficient\": (22, 1),\n",
" \"I_Coefficient\": (23, 1),\n",
" \"Minimum_Startup_Force\": (24, 2),\n",
" \"CW_Dead_Zone\": (26, 1),\n",
" \"CCW_Dead_Zone\": (27, 1),\n",
" \"Protection_Current\": (28, 2),\n",
" \"Angular_Resolution\": (30, 1),\n",
" \"Offset\": (31, 2),\n",
" \"Mode\": (33, 1),\n",
" \"Protective_Torque\": (34, 1),\n",
" \"Protection_Time\": (35, 1),\n",
" \"Overload_Torque\": (36, 1),\n",
" \"Speed_closed_loop_P_proportional_coefficient\": (37, 1),\n",
" \"Over_Current_Protection_Time\": (38, 1),\n",
" \"Velocity_closed_loop_I_integral_coefficient\": (39, 1),\n",
" \"Torque_Enable\": (40, 1),\n",
" \"Acceleration\": (41, 1),\n",
" \"Goal_Position\": (42, 2),\n",
" \"Goal_Time\": (44, 2),\n",
" \"Goal_Speed\": (46, 2),\n",
" \"Torque_Limit\": (48, 2),\n",
" \"Lock\": (55, 1),\n",
" \"Present_Position\": (56, 2),\n",
" \"Present_Speed\": (58, 2),\n",
" \"Present_Load\": (60, 2),\n",
" \"Present_Voltage\": (62, 1),\n",
" \"Present_Temperature\": (63, 1),\n",
" \"Status\": (65, 1),\n",
" \"Moving\": (66, 1),\n",
" \"Present_Current\": (69, 2),\n",
" # Not in the Memory Table\n",
" \"Maximum_Acceleration\": (85, 2),\n",
"}\n",
"\n",
"import time\n",
"\n",
"# Assuming STS_SERIES_CONTROL_TABLE is defined globally\n",
"\n",
"def print_all_params(robot):\n",
" \"\"\"\n",
" Reads all parameters from the STS_SERIES_CONTROL_TABLE and prints their values.\n",
" \"\"\"\n",
" for param in STS_SERIES_CONTROL_TABLE.keys():\n",
" try:\n",
" val = robot.arm_bus.read(param)\n",
" print(f\"{param} = {val}\")\n",
" except Exception as e:\n",
" print(f\"{param} read failed: {e}\")\n",
"\n",
"\n",
"# Example usage:\n",
"print_all_params(robot)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"probably max input voltage, we can also look at temperature and "
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Acceleration Read: [20 20]\n"
]
}
],
"source": [
"\n",
"print(\"Acceleration Read:\", robot.arm_bus.read(\"Acceleration\"))"
]
},
{
"cell_type": "code",
"execution_count": 37,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"LED_Alarm_Condition\", 2, [\"elbow_flex\"])"
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"Acceleration\", 20, [\"elbow_flex\"])\n",
"robot.arm_bus.write(\"Acceleration\", 100, [\"wrist_yaw\"])"
]
},
{
"cell_type": "code",
"execution_count": 73,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"Goal_Position\", [1000, 1000], [\"elbow_flex\", \"wrist_yaw\"])\n",
"time.sleep(2)\n",
"robot.arm_bus.write(\"Goal_Position\", [2000, 2000], [\"elbow_flex\", \"wrist_yaw\"])"
]
},
{
"cell_type": "code",
"execution_count": 68,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [0]\n",
"Elbow Flex Current: [3]\n",
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [2]\n",
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [2]\n",
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [1]\n",
"Elbow Flex Current: [0]\n"
]
},
{
"ename": "KeyboardInterrupt",
"evalue": "",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
"Cell \u001b[0;32mIn[68], line 25\u001b[0m\n\u001b[1;32m 22\u001b[0m time\u001b[38;5;241m.\u001b[39msleep(\u001b[38;5;241m2\u001b[39m)\n\u001b[1;32m 23\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 24\u001b[0m \u001b[38;5;66;03m# If current is zero, hold at pos_a for a bit\u001b[39;00m\n\u001b[0;32m---> 25\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\n",
"\u001b[0;31mKeyboardInterrupt\u001b[0m: "
]
}
],
"source": [
"import time\n",
"\n",
"# Enable torque on elbow_flex\n",
"robot.arm_bus.write(\"Torque_Enable\", 1, [\"elbow_flex\"])\n",
"\n",
"pos_a = 2500\n",
"pos_b = 1000\n",
"\n",
"robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n",
"time.sleep(2)\n",
"\n",
"while True:\n",
" current_val = robot.arm_bus.read(\"Present_Current\", \"elbow_flex\")\n",
" print(\"Elbow Flex Current:\", current_val)\n",
" \n",
" # If the servo is under non-zero load/current, switch position\n",
" if current_val > 1:\n",
" robot.arm_bus.write(\"Goal_Position\", pos_b, [\"elbow_flex\"])\n",
" time.sleep(2)\n",
" # Go back to pos_a again\n",
" robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n",
" time.sleep(2)\n",
" else:\n",
" # If current is zero, hold at pos_a for a bit\n",
" time.sleep(1)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\"Acceleration\" = 0, infinitely fast\n",
"\"Acceleration\" = 20 slow\n",
"elbow_flex is the LED one (4)\n",
"\n",
"robot.arm_bus.write(\"LED_Alarm_Condition\", 2, [\"elbow_flex\"]) #on constantly\n",
"robot.arm_bus.write(\"LED_Alarm_Condition\", 1, [\"elbow_flex\"]) #beeping\n",
"robot.arm_bus.write(\"LED_Alarm_Condition\", 0, [\"elbow_flex\"]) #off\n",
"\n",
"\"Max_Torque_Limit\": (16, 2), is what i have o play around with or \"Protective_Torque\": (37, 1), maybe\n",
"\n",
"robot.arm_bus.write(\"Torque_Enable\", 1, [\"elbow_flex\"]) 1 can move 0 cant move\n",
"\n",
"robot.arm_bus.write(\"Torque_Limit\", 300, [\"elbow_flex\"]) #how strong/weak the servo is. 0 makes it so that it cannot move basically, but i'd like to have that value change honestly and for it to be waeaker\n",
"\n",
"robot.arm_bus.write(\"Torque_Limit\", 20,[\"elbow_flex\"]) 20 in ordre to get some motion\n",
"\n",
"default is 1000\n",
"\n",
"robot.arm_bus.write(\"Goal_Speed\", -s, [\"elbow_flex\"]) #changes how fast the servo moves when going to the target, does not make it move with a specific speed "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"import time\n",
"\n",
"# Enable torque on elbow_flex\n",
"robot.arm_bus.write(\"Torque_Enable\", 1, [\"elbow_flex\"])\n",
"\n",
"pos_a = 1000\n",
"pos_b = 2500\n",
"\n",
"robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n",
"time.sleep(2)\n",
"\n",
"while True:\n",
" current_val = robot.arm_bus.read(\"Present_Current\", \"elbow_flex\")\n",
" print(\"Elbow Flex Current:\", current_val)\n",
" \n",
" # If the servo is under non-zero load/current, switch position\n",
" if current_val > 1:\n",
" robot.arm_bus.write(\"Goal_Position\", pos_b, [\"elbow_flex\"])\n",
" time.sleep(2)\n",
" # Go back to pos_a again\n",
" robot.arm_bus.write(\"Goal_Position\", pos_a, [\"elbow_flex\"])\n",
" time.sleep(2)\n",
" else:\n",
" # If current is zero, hold at pos_a for a bit\n",
" time.sleep(1)\n",
"\n",
"\n",
"so if current is larger than x then you disable it \n",
"\n"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"robot.arm_bus.write(\"LED_Alarm_Condition\", 2, [\"elbow_flex\"])"
]
},
{
"cell_type": "code",
"execution_count": 43,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[0]\n",
"[0]\n",
"[2]\n",
"[4]\n",
"[0]\n",
"[0]\n",
"[0]\n"
]
},
{
"ename": "KeyboardInterrupt",
"evalue": "",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
"Cell \u001b[0;32mIn[43], line 3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mwhile\u001b[39;00m \u001b[38;5;28;01mTrue\u001b[39;00m:\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28mprint\u001b[39m(robot\u001b[38;5;241m.\u001b[39marm_bus\u001b[38;5;241m.\u001b[39mread(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mPresent_Current\u001b[39m\u001b[38;5;124m\"\u001b[39m, [\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124melbow_flex\u001b[39m\u001b[38;5;124m\"\u001b[39m]))\n\u001b[0;32m----> 3\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m1\u001b[39;49m\u001b[43m)\u001b[49m\n",
"\u001b[0;31mKeyboardInterrupt\u001b[0m: "
]
}
],
"source": [
"while True:\n",
" print(robot.arm_bus.read(\"Present_Current\", [\"elbow_flex\"]))\n",
" time.sleep(1)"
]
},
{
"cell_type": "code",
"execution_count": 47,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Max_Voltage_Limit = [160 140 140 160 140 140 80]\n",
"Min_Angle_Limit = [ 650 1300 1300 1200 600 1725 0]\n",
"Max_Angle_Limit = [2600 2050 2800 2500 4096 2250 4095]\n",
"Max_Temperature_Limit = [80 70 70 80 70 70 70]\n",
"Acceleration = [20 20 20 20 20 20 20]\n",
"Torque_Limit = [1000 1000 1000 200 1000 1000 1000]\n",
"Minimum_Startup_Force = [15 16 16 12 16 16 16]\n",
"Protection_Current = [310 310 310 310 310 310 500]\n"
]
}
],
"source": [
"import time\n",
"\n",
"def print_important_params(robot):\n",
"\n",
" # Example parameters you mentioned; adjust as needed\n",
" param_list = [\n",
" \"Max_Voltage_Limit\",\n",
" \"Min_Angle_Limit\",\n",
" \"Max_Angle_Limit\",\n",
" \"Max_Temperature_Limit\",\n",
" \"Acceleration\", # or \"Maximum_Acceleration\" if you prefer that register\n",
" \"Torque_Limit\", # or \"Max_Torque_Limit\" if your table uses that\n",
" \"Minimum_Startup_Force\",\n",
" \"Protection_Current\",\n",
" ]\n",
" \n",
" for param in param_list:\n",
" try:\n",
" val = robot.arm_bus.read(param)\n",
" print(f\"{param} = {val}\")\n",
" except Exception as e:\n",
" print(f\"{param} read failed: {e}\")\n",
"\n",
"# -------------------------------\n",
"# Example usage\n",
"\n",
"print_important_params(robot)\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "lerobot",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.16"
}
},
"nbformat": 4,
"nbformat_minor": 2
}

27
examples/test_torque/test_torque.py Normal file
View File

@ -0,0 +1,27 @@
import time
from hopejr import HopeJuniorRobot
def main():
# Instantiate and connect to the robot
robot = HopeJuniorRobot()
robot.connect()
# Example read of the current position
print("Present Position:", robot.arm_bus.read("Present_Position"))
# Enable torque and set acceleration
robot.arm_bus.write("Torque_Enable", 1)
robot.arm_bus.write("Acceleration", 20)
print("Acceleration Read:", robot.arm_bus.read("Acceleration"))
# Move elbow_flex and wrist_yaw a few times
robot.arm_bus.write("Goal_Position", [1000, 1000], ["elbow_flex", "wrist_yaw"])
time.sleep(2)
robot.arm_bus.write("Goal_Position", [1500, 1500], ["elbow_flex", "wrist_yaw"])
time.sleep(2)
robot.arm_bus.write("Goal_Position", [1000, 1000], ["elbow_flex", "wrist_yaw"])
if __name__ == "__main__":
main()

49
examples/test_torque/var_table.json Normal file
View File

@ -0,0 +1,49 @@
STS_SERIES_CONTROL_TABLE = {
"Model": (3, 2),
"ID": (5, 1),
"Baud_Rate": (6, 1),
"Return_Delay": (7, 1),
"Response_Status_Level": (8, 1),
"Min_Angle_Limit": (9, 2),
"Max_Angle_Limit": (11, 2),
"Max_Temperature_Limit": (13, 1),
"Max_Voltage_Limit": (14, 1),
"Min_Voltage_Limit": (15, 1),
"Max_Torque_Limit": (16, 2),
"Phase": (18, 1),
"Unloading_Condition": (19, 1),
"LED_Alarm_Condition": (20, 1),
"P_Coefficient": (21, 1),
"D_Coefficient": (22, 1),
"I_Coefficient": (23, 1),
"Minimum_Startup_Force": (24, 2),
"CW_Dead_Zone": (26, 1),
"CCW_Dead_Zone": (27, 1),
"Protection_Current": (28, 2),
"Angular_Resolution": (30, 1),
"Offset": (31, 2),
"Mode": (33, 1),
"Protective_Torque": (34, 1),
"Protection_Time": (35, 1),
"Overload_Torque": (36, 1),
"Speed_closed_loop_P_proportional_coefficient": (37, 1),
"Over_Current_Protection_Time": (38, 1),
"Velocity_closed_loop_I_integral_coefficient": (39, 1),
"Torque_Enable": (40, 1),
"Acceleration": (41, 1),
"Goal_Position": (42, 2),
"Goal_Time": (44, 2),
"Goal_Speed": (46, 2),
"Torque_Limit": (48, 2),
"Lock": (55, 1),
"Present_Position": (56, 2),
"Present_Speed": (58, 2),
"Present_Load": (60, 2),
"Present_Voltage": (62, 1),
"Present_Temperature": (63, 1),
"Status": (65, 1),
"Moving": (66, 1),
"Present_Current": (69, 2),
# Not in the Memory Table
"Maximum_Acceleration": (85, 2),
}

16
examples/test_torque/why it fails Normal file
View File

@ -0,0 +1,16 @@
First check that kicks in is current:
Protection_Current (310) amperes or sth
Present_Current, compared against protection crrent
Over_Current_Protection_Time, how long until you shut it down
make a quick update about this
variables of interest are
Max_Torque_Limit = 1000,
Present_Load = 1000-something, which triggered the overload torque mechanism
Overload_Torque = 80, how much of the max torque limit do we allow?
Protection_Time = 200, after how long do we set Torque_Enable to 1? *not true lol
Protective_Torque = 20, after we trigger the safety mechanism, how much torque do we allow the motor to have?
theres actually no temperature or voltage check that the feetechs perform, the only two are current and torque, which works like i said above

BIN
nice were not even close .png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 43 KiB

174
servo_log.txt Normal file
View File

@ -0,0 +1,174 @@
Logging parameters for servo: shoulder_pitch
Duration: 2s, Interval: 0.1s
==================================================
Timestamp: 0.000
Present_Current = [19]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [123]
Present_Load = [123]
Present_Position = [1803]
--------------------------------------------------
Timestamp: 0.105
Present_Current = [51]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [121]
Present_Load = [207]
Present_Position = [1742]
--------------------------------------------------
Timestamp: 0.211
Present_Current = [130]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [120]
Present_Load = [363]
Present_Position = [1668]
--------------------------------------------------
Timestamp: 0.316
Present_Current = [192]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [120]
Present_Load = [471]
Present_Position = [1564]
--------------------------------------------------
Timestamp: 0.422
Present_Current = [177]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [120]
Present_Load = [503]
Present_Position = [1428]
--------------------------------------------------
Timestamp: 0.528
Present_Current = [294]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [118]
Present_Load = [615]
Present_Position = [1282]
--------------------------------------------------
Timestamp: 0.633
Present_Current = [360]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [118]
Present_Load = [671]
Present_Position = [1141]
--------------------------------------------------
Timestamp: 0.739
Present_Current = [360]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [118]
Present_Load = [639]
Present_Position = [1012]
--------------------------------------------------
Timestamp: 0.845
Present_Current = [348]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [118]
Present_Load = [615]
Present_Position = [907]
--------------------------------------------------
Timestamp: 0.951
Present_Current = [407]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [118]
Present_Load = [643]
Present_Position = [830]
--------------------------------------------------
Timestamp: 1.057
Present_Current = [479]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [38]
Present_Voltage = [117]
Present_Load = [679]
Present_Position = [776]
--------------------------------------------------
Timestamp: 1.163
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [947]
--------------------------------------------------
Timestamp: 1.269
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [1136]
--------------------------------------------------
Timestamp: 1.375
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [1438]
--------------------------------------------------
Timestamp: 1.481
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [1782]
--------------------------------------------------
Timestamp: 1.587
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [1949]
--------------------------------------------------
Timestamp: 1.693
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [2042]
--------------------------------------------------
Timestamp: 1.799
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [2049]
--------------------------------------------------
Timestamp: 1.906
Present_Current = [0]
Protection_Current = [310]
Over_Current_Protection_Time = [50]
Present_Temperature = [37]
Present_Voltage = [122]
Present_Load = [0]
Present_Position = [1997]
--------------------------------------------------

1107
shoulder_pitch_fail.txt Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
shoulder_pitch_log_200ms.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 64 KiB

BIN
shoulder_pitch_log_50ms.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 66 KiB

BIN
shoulder_pitch_parameters_plot.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 66 KiB