ydy0615/lerobot-clone
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-06-04 04:41:24 +00:00
Code Issues Packages Projects Releases Wiki Activity

remove settings add record

Browse Source
This commit is contained in:
Pepijn
2025-07-16 16:06:53 +02:00
parent 05a2316a63
commit 75ce54e212
2 changed files with 173 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
src/lerobot/bi_teleoperate.py
View File

@@ -14,12 +14,12 @@ ESC_CLR_EOL = "\x1b[K"
CURSOR_UP = "\x1b[F"
follower_cfg = SO101FollowerTConfig(
port="/dev/tty.usbmodem58760431551",
port="/dev/tty.usbmodem58760432961",
id="follower_arm_torque",
)
leader_cfg = SO101FollowerTConfig(
port="/dev/tty.usbmodem58760428721",
port="/dev/tty.usbmodem58760432571",
id="leader_arm_torque",
)
@@ -45,7 +45,7 @@ while True:
dt = tic - tic_prev
tic_prev = tic
if dt <= 0.0:
dt = 1e-3 # avoid div-by-zero on very first pass
dt = 0.01 # avoid div-by-zero
# Simplified model-based bilateral control
tau_cmd_f, tau_cmd_l = [], []
@@ -54,35 +54,29 @@ while True:
# 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}
acc_f = {j: obs_f[f"{j}.acc"] for j in follower.bus.motors}
tau_meas_f = {j: obs_f[f"{j}.tau_meas"] for j in follower.bus.motors}
tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in follower.bus.motors}
pos_l = {j: obs_l[f"{j}.pos"] for j in leader.bus.motors}
vel_l = {j: obs_l[f"{j}.vel"] for j in leader.bus.motors}
acc_l = {j: obs_l[f"{j}.acc"] for j in leader.bus.motors}
tau_meas_l = {j: obs_l[f"{j}.tau_meas"] for j in leader.bus.motors}
# Compute reaction torques using model-based approach
tau_reaction_f = follower._compute_model_based_disturbance(pos_f, vel_f, acc_f, tau_meas_f)
tau_reaction_l = leader._compute_model_based_disturbance(pos_l, vel_l, acc_l, tau_meas_l)
tau_reaction_l = {j: obs_l[f"{j}.effort"] for j in leader.bus.motors}
# Joint-specific control gains for better tracking
Kp_gains = follower.kp_gains
Kd_gains = follower.kd_gains
Kf_gains = follower.kf_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
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
+ Kf_gains[j] * (-tau_reaction_l[j] - tau_reaction_f[j]) # Force reflection
kp_gains[j] * (pos_l[j] - pos_f[j]) # Position tracking
+ kd_gains[j] * (vel_l[j] - vel_f[j]) # Velocity damping
+ kf_gains[j] * (-tau_reaction_l[j] - tau_reaction_f[j]) # Force reflection
for j in follower.bus.motors
]
tau_cmd_l = [
Kp_gains[j] * (pos_f[j] - pos_l[j]) # Position tracking
+ Kd_gains[j] * (vel_f[j] - vel_l[j]) # Velocity damping
+ Kf_gains[j] * (-tau_reaction_f[j] - tau_reaction_l[j]) # Force reflection
kp_gains[j] * (pos_f[j] - pos_l[j]) # Position tracking
+ kd_gains[j] * (vel_f[j] - vel_l[j]) # Velocity damping
+ kf_gains[j] * (-tau_reaction_f[j] - tau_reaction_l[j]) # Force reflection
for j in leader.bus.motors
]
@@ -90,7 +84,6 @@ while True:
for i, j in enumerate(follower.bus.motors):
# Store debug info
debug_info_f[j] = {
"τ_measured": tau_meas_f[j],
"τ_reaction": tau_reaction_f[j],
"τ_ref": tau_cmd_f[i],
"θ_err": pos_l[j] - pos_f[j],
@@ -98,7 +91,6 @@ while True:
"τ_err": -tau_reaction_l[j] - tau_reaction_f[j],
}
debug_info_l[j] = {
"τ_measured": tau_meas_l[j],
"τ_reaction": tau_reaction_l[j],
"τ_ref": tau_cmd_l[i],
"θ_err": pos_f[j] - pos_l[j],
@@ -137,36 +129,28 @@ while True:
lines = [f"Loop {hz:6.1f} Hz Δt {dt * 1e3:5.2f} ms"]
lines.append("=" * 106)
lines.append("LEADER ARM TORQUE ANALYSIS:")
lines.append(f"{'Joint':<13}{'Pos':>8}{'React':>6}{'Meas':>6}{'Cmd':>6}")
lines.append(f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}")
lines.append(f"{'Joint':<13}{'Pos':>8}{'React':>6}{'Cmd':>6}")
lines.append(f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}")
lines.append("-" * 86)
for i, j in enumerate(leader.bus.motors):
debug_l = debug_info_l[j]
lines.append(
f"{j:<13s}"
f"{math.degrees(pos_l[j]):+8.1f}"
f"{debug_l['τ_reaction']:+6.2f}"
f"{debug_l['τ_measured']:+6.2f}"
f"{tau_cmd_l[i]:+6.2f}"
f"{j:<13s}{math.degrees(pos_l[j]):+8.1f}{debug_l['τ_reaction']:+6.2f}{tau_cmd_l[i]:+6.2f}"
)
lines.append("")
lines.append("FOLLOWER ARM TORQUE ANALYSIS:")
lines.append(f"{'Joint':<13}{'Pos':>8}{'React':>6}{'Meas':>6}{'Cmd':>6}")
lines.append(f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}{'(Nm)':>6}")
lines.append(f"{'Joint':<13}{'Pos':>8}{'React':>6}{'Cmd':>6}")
lines.append(f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}")
lines.append("-" * 86)
for i, j in enumerate(follower.bus.motors):
debug_f = debug_info_f[j]
lines.append(
f"{j:<13s}"
f"{math.degrees(pos_f[j]):+8.1f}"
f"{debug_f['τ_reaction']:+6.2f}"
f"{debug_f['τ_measured']:+6.2f}"
f"{tau_cmd_f[i]:+6.2f}"
f"{j:<13s}{math.degrees(pos_f[j]):+8.1f}{debug_f['τ_reaction']:+6.2f}{tau_cmd_f[i]:+6.2f}"
)
lines.append("")
@@ -184,7 +168,7 @@ while True:
lines.append("Force = Kf × (reflect_other_robot - React) (telepresence)")
lines.append("Frict = b_visc×ω + f_coulomb×sign(ω) (transparency)")
lines.append(
f"Joint Gains: shoulder_pan Kp={Kp_gains['shoulder_pan']:.1f} | shoulder_pan Kd={Kd_gains['shoulder_pan']:.1f} | shoulder_pan Kf={Kf_gains['shoulder_pan']:.1f}"
f"Joint Gains: shoulder_pan Kp={kp_gains['shoulder_pan']:.1f} | shoulder_pan Kd={kd_gains['shoulder_pan']:.1f} | shoulder_pan Kf={kf_gains['shoulder_pan']:.1f}"
)
lines.append(
f"Friction Comp, Viscous: {follower.friction_viscous['shoulder_pan']:.3f} | Coulomb: {follower.friction_coulomb['shoulder_pan']:.3f} (robot-class)"

161
src/lerobot/record.py
View File

@@ -33,6 +33,23 @@ python -m lerobot.record \
# <- Policy optional if you want to record with a policy \
# --policy.path=${HF_USER}/my_policy \
```
Example with bilateral teleoperation:
```shell
python -m lerobot.record \
--robot.type=so101_follower_t \
--robot.port=/dev/tty.usbmodem58760431551 \
--robot.id=follower_arm_torque \
--dataset.repo_id=pepijn/bilateral-teleop-test \
--dataset.num_episodes=5 \
--dataset.single_task="Wipe the table" \
--biteleop=true \
--teleop.type=so101_follower_t \
--teleop.port=/dev/tty.usbmodem58760428721 \
--teleop.id=leader_arm_torque \
--display_data=true
```
"""
import logging
@@ -62,7 +79,9 @@ from lerobot.robots import ( # noqa: F401
make_robot_from_config,
so100_follower,
so101_follower,
so101_follower_torque,
)
from lerobot.robots.so101_follower_torque import SO101FollowerT
from lerobot.teleoperators import ( # noqa: F401
Teleoperator,
TeleoperatorConfig,
@@ -133,7 +152,7 @@ class RecordConfig:
robot: RobotConfig
dataset: DatasetRecordConfig
# Whether to control the robot with a teleoperator
teleop: TeleoperatorConfig | None = None
teleop: TeleoperatorConfig | RobotConfig | None = None
# Whether to control the robot with a policy
policy: PreTrainedConfig | None = None
# Display all cameras on screen
@@ -142,6 +161,8 @@ class RecordConfig:
play_sounds: bool = True
# Resume recording on an existing dataset.
resume: bool = False
# Enable bilateral teleoperation with force feedback
biteleop: bool = False
def __post_init__(self):
# HACK: We parse again the cli args here to get the pretrained path if there was one.
@@ -166,7 +187,10 @@ def record_loop(
events: dict,
fps: int,
dataset: LeRobotDataset | None = None,
teleop: Teleoperator | List[Teleoperator] | None = None,
teleop: Teleoperator
| List[Teleoperator]
| Robot
| None = None, # Allow Robot type for bilateral teleoperation
policy: PreTrainedPolicy | None = None,
control_time_s: int | None = None,
single_task: str | None = None,
@@ -176,6 +200,14 @@ 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:
if not isinstance(robot, SO101FollowerT) or not isinstance(teleop, SO101FollowerT):
raise ValueError(
"Bilateral teleoperation requires both robot and teleop to be of type SO101FollowerT"
)
logging.info("Bilateral teleoperation mode enabled")
teleop_arm = teleop_keyboard = None
if isinstance(teleop, list):
teleop_keyboard = next((t for t in teleop if isinstance(t, KeyboardTeleop)), None)
@@ -199,7 +231,12 @@ def record_loop(
timestamp = 0
start_episode_t = time.perf_counter()
while timestamp < control_time_s:
# For controlling rerun logging frequency
loop_count = 0
rerun_log_freq = max(1, int(fps / 10)) # Log at 10Hz
while control_time_s is not None and timestamp < control_time_s:
start_loop_t = time.perf_counter()
if events["exit_early"]:
@@ -211,7 +248,73 @@ def record_loop(
if policy is not None or dataset is not None:
observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
if policy is not None:
if (
biteleop
and isinstance(robot, SO101FollowerT)
and isinstance(teleop, SO101FollowerT)
and policy is None
):
# Bilateral teleoperation control
# 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}
pos_l = {j: obs_l[f"{j}.pos"] for j in teleop.bus.motors}
vel_l = {j: obs_l[f"{j}.vel"] for j in teleop.bus.motors}
tau_reaction_l = {j: obs_l[f"{j}.effort"] for j in teleop.bus.motors}
# Get control gains from robot
kp_gains = robot.kp_gains
kd_gains = robot.kd_gains
kf_gains = robot.kf_gains
# Compute torque commands in one line using list comprehension
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
+ kf_gains[j] * (-tau_reaction_l[j] - tau_reaction_f[j]) # Force reflection
for j in robot.bus.motors
]
tau_cmd_l = [
kp_gains[j] * (pos_f[j] - pos_l[j]) # Position tracking
+ kd_gains[j] * (vel_f[j] - vel_l[j]) # Velocity damping
+ kf_gains[j] * (-tau_reaction_f[j] - tau_reaction_l[j]) # Force reflection
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)
# For bilateral teleoperation, create custom observation and action for dataset
# Observation: follower position and reaction torque
bilateral_observation = {}
for j in robot.bus.motors:
bilateral_observation[f"{j}.pos"] = pos_f[j]
bilateral_observation[f"{j}.effort"] = tau_reaction_f[j]
# Action: leader position and interaction torque
bilateral_action = {}
for j in teleop.bus.motors:
bilateral_action[f"{j}.pos"] = pos_l[j]
bilateral_action[f"{j}.effort"] = tau_reaction_l[j]
# Override the observation_frame and action for dataset recording
if dataset is not None:
observation_frame = build_dataset_frame(
dataset.features, bilateral_observation, prefix="observation"
)
# Store bilateral_action to be used later when building action_frame
action = bilateral_action
elif policy is not None:
action_values = predict_action(
observation_frame,
policy,
@@ -221,9 +324,9 @@ def record_loop(
robot_type=robot.robot_type,
)
action = {key: action_values[i].item() for i, key in enumerate(robot.action_features)}
elif policy is None and isinstance(teleop, Teleoperator):
elif policy is None and isinstance(teleop, Teleoperator) and not biteleop:
action = teleop.get_action()
elif policy is None and isinstance(teleop, list):
elif policy is None and isinstance(teleop, list) and not biteleop:
# TODO(pepijn, steven): clean the record loop for use of multiple robots (possibly with pipeline)
arm_action = teleop_arm.get_action()
arm_action = {f"arm_{k}": v for k, v in arm_action.items()}
@@ -245,28 +348,66 @@ def record_loop(
sent_action = robot.send_action(action)
if dataset is not None:
action_frame = build_dataset_frame(dataset.features, sent_action, prefix="action")
# For bilateral teleoperation, use the bilateral_action (leader pos & torque)
# For other modes, use sent_action as usual
if (
biteleop
and isinstance(robot, SO101FollowerT)
and isinstance(teleop, SO101FollowerT)
and policy is None
):
action_frame = build_dataset_frame(dataset.features, action, prefix="action")
else:
action_frame = build_dataset_frame(dataset.features, sent_action, prefix="action")
frame = {**observation_frame, **action_frame}
dataset.add_frame(frame, task=single_task)
if display_data:
if display_data and loop_count % rerun_log_freq == 0:
log_rerun_data(observation, action)
dt_s = time.perf_counter() - start_loop_t
busy_wait(1 / fps - dt_s)
timestamp = time.perf_counter() - start_episode_t
loop_count += 1
@parser.wrap()
def record(cfg: RecordConfig) -> LeRobotDataset:
init_logging()
logging.info(pformat(asdict(cfg)))
if cfg.display_data:
_init_rerun(session_name="recording")
robot = make_robot_from_config(cfg.robot)
teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None
# For bilateral teleoperation, create teleop as a robot instance
if cfg.biteleop:
print("Bilateral teleoperation enabled")
# For bilateral teleoperation, both arms must be SO101FollowerT robots
from lerobot.robots.so101_follower_torque.config_so101_follower_t import SO101FollowerTConfig
# Check if teleop config has the right type
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")
teleop_robot_config = SO101FollowerTConfig(
port=port,
id=getattr(cfg.teleop, "id", "leader_arm_torque"),
cameras=getattr(cfg.teleop, "cameras", {}),
disable_torque_on_disconnect=getattr(cfg.teleop, "disable_torque_on_disconnect", True),
)
teleop = SO101FollowerT(teleop_robot_config)
else:
teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None
action_features = hw_to_dataset_features(robot.action_features, "action", cfg.dataset.video)
obs_features = hw_to_dataset_features(robot.observation_features, "observation", cfg.dataset.video)
@@ -320,6 +461,7 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
control_time_s=cfg.dataset.episode_time_s,
single_task=cfg.dataset.single_task,
display_data=cfg.display_data,
biteleop=cfg.biteleop,
)
# Execute a few seconds without recording to give time to manually reset the environment
@@ -336,6 +478,7 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
control_time_s=cfg.dataset.reset_time_s,
single_task=cfg.dataset.single_task,
display_data=cfg.display_data,
biteleop=cfg.biteleop,
)
if events["rerecord_episode"]: