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

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