diff --git a/src/lerobot/bi_teleoperate.py b/src/lerobot/bi_teleoperate.py index be103eaf2..7f1014d74 100644 --- a/src/lerobot/bi_teleoperate.py +++ b/src/lerobot/bi_teleoperate.py @@ -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:") diff --git a/src/lerobot/motors/feetech/feetech.py b/src/lerobot/motors/feetech/feetech.py index 54b0ddfbe..abfc6176e 100644 --- a/src/lerobot/motors/feetech/feetech.py +++ b/src/lerobot/motors/feetech/feetech.py @@ -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) diff --git a/src/lerobot/motors/feetech/tables.py b/src/lerobot/motors/feetech/tables.py index 4ccecc33d..0c5156c6f 100644 --- a/src/lerobot/motors/feetech/tables.py +++ b/src/lerobot/motors/feetech/tables.py @@ -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) diff --git a/src/lerobot/record.py b/src/lerobot/record.py index 3deb665fb..2f24c6a27 100644 --- a/src/lerobot/record.py +++ b/src/lerobot/record.py @@ -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") diff --git a/src/lerobot/replay.py b/src/lerobot/replay.py index 41fa87a06..ffc2b2f82 100644 --- a/src/lerobot/replay.py +++ b/src/lerobot/replay.py @@ -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 diff --git a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py index a908033ad..d0c116116 100644 --- a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py +++ b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py @@ -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)