admin/lerobot
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-07-06 09:37:06 +00:00
Code Issues Packages Projects Releases Wiki Activity

Small fixes

Browse Source
This commit is contained in:
Pepijn
2025-07-28 09:13:01 +02:00
parent 2e3c116fad
commit ac1c2454c5
6 changed files with 14 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/bi_teleoperate.py
+3 -13
View File
@@ -28,10 +28,8 @@ leader = SO101FollowerT(leader_cfg)
follower.connect()
leader.connect()
# Initialize Rerun for visualization
_init_rerun("bilateral_teleoperation")
print("Starting 4-channel bilateral teleoperation")
first_print = True
loop_count = 0
@@ -47,11 +45,9 @@ while True:
if dt <= 0.0:
dt = 0.01 # avoid div-by-zero
# Simplified model-based bilateral control
tau_cmd_f, tau_cmd_l = [], []
debug_info_f, debug_info_l = {}, {}
# Collect data for all motors
pos_f = {j: obs_f[f"{j}.pos"] for j in follower.bus.motors}
vel_f = {j: obs_f[f"{j}.vel"] for j in follower.bus.motors}
tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in follower.bus.motors}
@@ -60,12 +56,12 @@ while True:
vel_l = {j: obs_l[f"{j}.vel"] for j in leader.bus.motors}
tau_reaction_l = {j: obs_l[f"{j}.effort"] for j in leader.bus.motors}
# Joint-specific control gains for better tracking
# Joint-specific control gains
kp_gains = follower.kp_gains
kd_gains = follower.kd_gains
kf_gains = follower.kf_gains
# Compute torque commands in one line using list comprehension
# Compute torque commands
tau_cmd_f = [
kp_gains[j] * (pos_l[j] - pos_f[j]) # Position tracking
+ kd_gains[j] * (vel_l[j] - vel_f[j]) # Velocity damping
@@ -80,9 +76,8 @@ while True:
for j in leader.bus.motors
]
# Store interaction torques and debug info
# Store debug info
for i, j in enumerate(follower.bus.motors):
# Store debug info
debug_info_f[j] = {
"τ_reaction": tau_reaction_f[j],
"τ_ref": tau_cmd_f[i],
@@ -102,30 +97,25 @@ while True:
follower.send_action({f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(follower.bus.motors)})
leader.send_action({f"{m}.effort": tau_cmd_l[i] for i, m in enumerate(leader.bus.motors)})
# Observation: follower side only (θ_f, ω_f, τ_ext)
observation = {
"follower_joint_angles": pos_f, # θ_f: current angles
"follower_angular_velocities": vel_f, # ω_f: current velocities
"follower_external_torques": tau_reaction_f, # τ_ext: measured minus deterministic components
}
# Action: leader targets (θ_leader[τ], ω_leader[τ], τ_leader[τ])
action = {
"leader_target_angles": pos_l, # θ_leader[τ]: absolute target angles
"leader_target_velocities": vel_l, # ω_leader[τ]: absolute target velocities
"leader_interaction_torques": tau_reaction_l, # τ_leader[τ]: cmd minus deterministic components
}
# Log data for visualization (100 Hz)
if loop_count % (FRQ // RERUN_HZ) == 0:
log_rerun_data(observation, action)
# Console diagnostics (10 Hz)
loop_count += 1
if loop_count % (FRQ // PRINT_HZ) == 0:
hz = 1.0 / dt
# Detailed torque analysis mode - LEADER
lines = [f"Loop {hz:6.1f} Hz Δt {dt * 1e3:5.2f} ms"]
lines.append("=" * 106)
lines.append("LEADER ARM TORQUE ANALYSIS:")
src/lerobot/motors/feetech/feetech.py
+1 -1
View File
@@ -224,7 +224,7 @@ class FeetechMotorsBus(MotorsBus):
for motor in self.motors:
# By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on
# the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
# self.write("Return_Delay_Time", motor, 0)
# self.write("Return_Delay_Time", motor, 0) # THIS DOES NOT WORK FOR HLS3625
# Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors.
if self.protocol_version == 0:
self.write("Maximum_Acceleration", motor, maximum_acceleration)
src/lerobot/motors/feetech/tables.py
-47
View File
@@ -357,50 +357,3 @@ MODEL_PROTOCOL = {
"scs0009": 1,
"hls3625": 0, # Uses FT-SCS protocol
}
# HLS series special byte bit flag definitions
# These are used to interpret the bit flags in Phase, Status, Unloading_Condition, and LED_Alarm_Condition
# Phase byte (address 18) bit meanings:
# BIT0 (1): Current driving direction phase (0: forward, 1: reverse) - invalid in firmware 3.42+
# BIT1 (2): Current feedback direction phase (0: forward, 1: reverse)
# BIT2 (4): Drive bridge direction phase (0: forward, 1: reverse)
# BIT3 (8): Speed feedback direction phase (0: forward, 1: reverse)
# BIT4 (16): Angle feedback mode (0: single-circle angle, 1: full angle)
# BIT5 (32): Driver bridge configuration (0: independent H bridge, 1: integrated H bridge)
# BIT6 (64): PWM frequency (0: 24kHz, 1: 16kHz)
# BIT7 (128): Position feedback direction phase (0: forward, 1: reverse)
# Status byte (address 65) bit meanings (0: normal, 1: abnormal):
# BIT0 (1): Voltage status
# BIT1 (2): Magnetic coding state
# BIT2 (4): Temperature status
# BIT3 (8): Current status
# BIT4 (16): Reserved
# BIT5 (32): Load status
# BIT6 (64): Reserved
# BIT7 (128): Reserved
# Unloading_Condition byte (address 19) bit meanings (0: off, 1: on):
# BIT0 (1): Voltage protection
# BIT1 (2): Magnetic coding protection
# BIT2 (4): Overheating protection
# BIT3 (8): Overcurrent protection
# BIT4 (16): Reserved
# BIT5 (32): Load overload protection
# BIT6 (64): Reserved
# BIT7 (128): Reserved
# LED_Alarm_Condition byte (address 20) bit meanings (0: off, 1: on):
# BIT0 (1): Voltage alarm
# BIT1 (2): Magnetic coding alarm
# BIT2 (4): Overheating alarm
# BIT3 (8): Overcurrent alarm
# BIT4 (16): Reserved
# BIT5 (32): Load overload alarm
# BIT6 (64): Reserved
# BIT7 (128): Reserved
# Moving byte (address 66) bit meanings:
# BIT0 (1): Motor movement status (1: moving, 0: stopped)
# BIT1 (2): Target position status (0: reached target, 1: not reached target)
src/lerobot/record.py
+4 -19
View File
@@ -223,10 +223,7 @@ def record_loop(
events: dict,
fps: int,
dataset: LeRobotDataset | None = None,
teleop: Teleoperator
| List[Teleoperator]
| Robot
| None = None, # Allow Robot type for bilateral teleoperation
teleop: Teleoperator | List[Teleoperator] | Robot | None = None,
policy: PreTrainedPolicy | None = None,
control_time_s: int | None = None,
single_task: str | None = None,
@@ -236,7 +233,6 @@ def record_loop(
if dataset is not None and dataset.fps != fps:
raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).")
# Check if bilateral teleoperation is enabled and validate robot/teleop types
if biteleop and policy is None:
if not isinstance(robot, SO101FollowerT):
raise ValueError(
@@ -268,9 +264,8 @@ def record_loop(
timestamp = 0
start_episode_t = time.perf_counter()
# For controlling rerun logging frequency
loop_count = 0
rerun_log_freq = max(1, int(fps / 10)) # Log at 10Hz
rerun_log_freq = max(1, int(fps / 10))
while control_time_s is not None and timestamp < control_time_s:
start_loop_t = time.perf_counter()
@@ -290,11 +285,9 @@ def record_loop(
and isinstance(teleop, SO101FollowerT)
and policy is None
):
# Get observations from both arms
obs_f = observation # robot is the follower
obs_l = teleop.get_observation()
# Collect data for all motors
pos_f = {j: obs_f[f"{j}.pos"] for j in robot.bus.motors}
vel_f = {j: obs_f[f"{j}.vel"] for j in robot.bus.motors}
tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in robot.bus.motors}
@@ -309,7 +302,7 @@ def record_loop(
kd_gains = robot.kd_gains
kf_gains = robot.kf_gains
# Compute torque commands in one line using list comprehension
# Compute torque commands
tau_cmd_f = [
(
kp_gains[j] * (pos_l[j] - pos_f[j]) # Position tracking
@@ -328,7 +321,6 @@ def record_loop(
for j in teleop.bus.motors
]
# Send torque commands
action = {f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(robot.bus.motors)}
teleop_action = {f"{m}.effort": tau_cmd_l[i] for i, m in enumerate(teleop.bus.motors)}
teleop.send_action(teleop_action)
@@ -345,7 +337,6 @@ def record_loop(
# Override the observation_frame and action for dataset recording
if dataset is not None:
observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
# Store bilateral_action to be used later when building action_frame
action = bilateral_action
elif policy is not None and biteleop and isinstance(robot, SO101FollowerT):
@@ -357,18 +348,16 @@ def record_loop(
task=single_task,
robot_type=robot.robot_type,
)
# Read robot current state
pos_f = {j: observation[f"{j}.pos"] for j in robot.bus.motors}
vel_f = {j: observation[f"{j}.vel"] for j in robot.bus.motors}
tau_reaction_f = {j: observation[f"{j}.effort"] for j in robot.bus.motors}
# The model returns [pos1, pos2, …, vel1, vel2, …, tau1, tau2, …]
motors = robot.bus.motors # 6 joints in your case
motors = robot.bus.motors # 6 joints
pos_l, vel_l, neg_tau_reaction_l = split_interleaved_action(
action_values, motors
) # The model is trained and returns the effort already as negative: -tau_reaction_l
# Get control gains from the robot instance
kp, kd, kf = robot.kp_gains, robot.kd_gains, robot.kf_gains
# Compute torque command for the follower robot
@@ -394,7 +383,6 @@ def record_loop(
# Override the observation_frame and action for dataset recording
if dataset is not None:
observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
# Store bilateral_action to be used later when building action_frame
action = bilateral_action
elif policy is not None and not biteleop:
@@ -462,7 +450,6 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
robot = make_robot_from_config(cfg.robot)
# For bilateral teleoperation, create teleop as a robot instance
if cfg.biteleop and cfg.teleop is not None:
print("Bilateral teleoperation enabled")
# For bilateral teleoperation, both arms must be SO101FollowerT robots
@@ -472,8 +459,6 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
if cfg.teleop.type != "so101_follower_t":
raise ValueError("Bilateral teleoperation requires teleop.type to be 'so101_follower_t'")
# Create teleop as SO101FollowerT robot instance
# Access attributes dynamically since they vary by teleop config type
port = getattr(cfg.teleop, "port", None)
if port is None:
raise ValueError("Bilateral teleoperation requires teleop.port to be specified")
src/lerobot/replay.py
-2
View File
@@ -119,13 +119,11 @@ def replay(cfg: ReplayConfig):
# Bilateral teleoperation
if cfg.biteleop:
# Get current follower robot observation
obs_f = robot.get_observation()
pos_f = {j: obs_f[f"{j}.pos"] for j in robot.bus.motors}
vel_f = {j: obs_f[f"{j}.vel"] for j in robot.bus.motors}
tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in robot.bus.motors}
# Get target leader state from the dataset
pos_l = {j: action_from_ds[f"{j}.pos"] for j in robot.bus.motors}
vel_l = {j: action_from_ds[f"{j}.vel"] for j in robot.bus.motors}
# The saved effort in dataset is -tau_reaction_l
src/lerobot/robots/so101_follower_torque/so101_follower_t.py
+6 -7
View File
@@ -49,7 +49,7 @@ class SO101FollowerT(Robot):
_KT_NM_PER_AMP: float = 0.814 # Torque constant Kt [N·m/A] #https://www.feetechrc.com/811177.html
_MAX_CURRENT_A: float = 4.0 # Safe driver limit
# Position gains [Nm/rad]
# Position gains
_KP_GAINS = {
"shoulder_pan": 5.0,
"shoulder_lift": 7.0,
@@ -59,7 +59,7 @@ class SO101FollowerT(Robot):
"gripper": 5.0,
}
# Velocity gains [Nm⋅s/rad]
# Velocity gains
_KD_GAINS = {
"shoulder_pan": 0.4,
"shoulder_lift": 0.6,
@@ -79,7 +79,7 @@ class SO101FollowerT(Robot):
"gripper": 0.05,
}
# Viscous friction coefficient [Nm⋅s/rad] per joint
# Viscous friction coefficient
_FRICTION_VISCOUS = {
"shoulder_pan": 0.05,
"shoulder_lift": 0.08,
@@ -89,7 +89,7 @@ class SO101FollowerT(Robot):
"gripper": 0.05,
}
# Coulomb/static friction [Nm] per joint
# Coulomb/static friction
_FRICTION_COULOMB = {
"shoulder_pan": 0.15,
"shoulder_lift": 0.25,
@@ -103,7 +103,6 @@ class SO101FollowerT(Robot):
super().__init__(config)
self.config = config
# Ensure calibration is loaded before creating the bus
if self.calibration_fpath.is_file() and not self.calibration:
self._load_calibration()
@@ -317,7 +316,7 @@ class SO101FollowerT(Robot):
tau_gravity = self._gravity_from_q(q_rad)
tau_inertia = self._inertia_from_q_dq(q_rad, dq_rad, ddq_rad)
# Compute disturbance: what's left after removing known dynamics
# Compute disturbance
tau_disturbance = {}
tau_friction = {}
for motor_name in self.bus.motors:
@@ -424,7 +423,7 @@ class SO101FollowerT(Robot):
self._pos_history[m].append(pos_rad[m])
self._time_history.append(t_now)
# Calculate raw velocity using finite differences
# Calculate raw velocity
vel_rad_raw = {}
if self._prev_pos_rad is None or self._prev_t is None:
vel_rad_raw = dict.fromkeys(pos_rad, 0.0)