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

Add friction to distribiutor estimation

Browse Source
This commit is contained in:
Pepijn
2025-07-08 15:08:35 +02:00
parent 5e27248bba
commit 25e22ea3ba
2 changed files with 110 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/bi_teleoperate.py
+32 -47
View File
@@ -50,6 +50,9 @@ first_print = True
loop_count = 0
tic_prev = time.perf_counter()
use_friction_comp = True # Enable/disable friction compensation
use_gravity_comp = True # Enable gravity compensation with corrected signs
# Initialize previous positions for velocity calculation
pos_f_prev = None
pos_l_prev = None
@@ -95,8 +98,13 @@ while True:
pos_l_prev = pos_l.copy()
# 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)
# Include friction in model-based disturbance for consistency
tau_reaction_f = follower._compute_model_based_disturbance(
pos_f, vel_f, acc_f, tau_meas_f, include_friction=use_friction_comp
)
tau_reaction_l = leader._compute_model_based_disturbance(
pos_l, vel_l, acc_l, tau_meas_l, include_friction=use_friction_comp
)
# Get gravity for feed-forward compensation
grav_f = follower._gravity_from_q(pos_f)
@@ -107,47 +115,14 @@ while True:
inertia_l = leader._inertia_from_q_dq(pos_l, vel_l, acc_l)
# Joint-specific control gains for better tracking
Kp_gains = { # Position gains [Nm/rad] - higher for proximal joints
"shoulder_pan": 7.0,
"shoulder_lift": 15.0,
"elbow_flex": 12.0,
"wrist_flex": 6.0,
"wrist_roll": 4.0,
"gripper": 2.0,
}
Kd_gains = { # Velocity gains [Nm⋅s/rad] - matched to position gains
"shoulder_pan": 0.4,
"shoulder_lift": 0.7,
"elbow_flex": 0.6,
"wrist_flex": 0.4,
"wrist_roll": 0.3,
"gripper": 0.2,
}
Kp_gains = follower.kp_gains # Use robot class properties
Kd_gains = follower.kd_gains # Use robot class properties
Kf = 0.05 # Force reflection gain (global)
use_friction_comp = True # Enable/disable friction compensation
use_gravity_comp = True # Set to True to enable gravity compensation
# Friction model: τ_friction = b_viscous * ω + fc_coulomb * sign(ω)
friction_viscous = { # Viscous friction coefficient [Nm⋅s/rad] per joint
"shoulder_pan": 0.02,
"shoulder_lift": 0.08,
"elbow_flex": 0.05,
"wrist_flex": 0.02,
"wrist_roll": 0.015,
"gripper": 0.01,
}
friction_coulomb = { # Coulomb friction [Nm] per joint - increased static friction
"shoulder_pan": 0.05,
"shoulder_lift": 0.30,
"elbow_flex": 0.20,
"wrist_flex": 0.10,
"wrist_roll": 0.06,
"gripper": 0.04,
}
# Friction model parameters from robot class
friction_viscous = follower.friction_viscous # Use robot class properties
friction_coulomb = follower.friction_coulomb # Use robot class properties
for j in follower.bus.motors:
# Bilateral control references (eqs. 5-7)
@@ -198,10 +173,16 @@ while True:
tau_cmd_l.append(tau_ref_l)
# Store interaction torques (cmd minus deterministic components) for model
tau_interaction_l[j] = tau_ref_l - tau_gravity_l - tau_inertia_l - tau_friction_l
tau_interaction_f[j] = -(
tau_meas_f[j] - tau_gravity_f - tau_inertia_f - tau_friction_f
) # Invert to match leader
if use_friction_comp:
# If friction compensation is enabled, friction is already removed in model-based disturbance
tau_interaction_l[j] = tau_ref_l - tau_gravity_l - tau_inertia_l - tau_friction_l
tau_interaction_f[j] = -tau_reaction_f[j] # Already has friction removed
else:
# If friction compensation is disabled, use original logic
tau_interaction_l[j] = tau_ref_l - tau_gravity_l - tau_inertia_l - tau_friction_l
tau_interaction_f[j] = -(
tau_meas_f[j] - tau_gravity_f - tau_inertia_f - tau_friction_f
) # Invert to match leader
# Store debug info
debug_info_f[j] = {
@@ -307,17 +288,21 @@ while True:
lines.append("• Cmd (command) = Final torque sent to motor")
lines.append("-" * 100)
lines.append("Cmd = Pos + Vel + Force + Grav + Inert + Frict")
lines.append("React = Meas - Grav - Inert (external forces)")
lines.append(
"React = Meas - Grav - Inert - Frict (external forces)"
if use_friction_comp
else "React = Meas - Grav - Inert (external forces)"
)
lines.append("Force = Kf × (reflect_other_robot - React) (telepresence)")
lines.append("Frict = b_visc×ω + f_coulomb×sign(ω) (transparency)")
lines.append(
f"Control Gains: Kp=joint-specific (position) | Kd=joint-specific (velocity) | Kf={Kf} (force reflection)"
f"Control Gains: Kp=robot-class (position) | Kd=robot-class (velocity) | Kf={Kf} (force reflection)"
)
lines.append(
f"Joint Gains: shoulder_pan Kp={Kp_gains['shoulder_pan']:.1f} | shoulder_lift Kp={Kp_gains['shoulder_lift']:.1f} | elbow_flex Kp={Kp_gains['elbow_flex']:.1f}"
)
lines.append(
f"Friction Comp: {'ON' if use_friction_comp else 'OFF'} | Viscous: {friction_viscous['shoulder_pan']:.3f} | Coulomb: {friction_coulomb['shoulder_pan']:.3f} (shoulder example)"
f"Friction Comp: {'ON' if use_friction_comp else 'OFF'} | Viscous: {friction_viscous['shoulder_pan']:.3f} | Coulomb: {friction_coulomb['shoulder_pan']:.3f} (robot-class)"
)
lines.append(
f"Gravity Comp: {'ON' if use_gravity_comp else 'OFF'} | Inertia Comp: {'ON' if use_gravity_comp else 'OFF'} | (Both disabled for testing)"
src/lerobot/robots/so101_follower_torque/so101_follower_t.py
+78 -2
View File
@@ -47,7 +47,45 @@ class SO101FollowerT(Robot):
_CURRENT_STEP_A: float = 6.5e-3 # 6.5 mA per register LSB #http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
_KT_NM_PER_AMP: float = 0.814 # Torque constant Kt [N·m/A] #https://www.feetechrc.com/811177.html
_MAX_CURRENT_A: float = 3.0 # Safe driver limit for this model
_MAX_CURRENT_A: float = 2.0 # Safe driver limit for this model
# Control gains for bilateral teleoperation
_KP_GAINS = { # Position gains [Nm/rad] - higher for proximal joints
"shoulder_pan": 7.0,
"shoulder_lift": 15.0,
"elbow_flex": 12.0,
"wrist_flex": 6.0,
"wrist_roll": 4.0,
"gripper": 2.0,
}
_KD_GAINS = { # Velocity gains [Nm⋅s/rad] - matched to position gains
"shoulder_pan": 0.4,
"shoulder_lift": 0.7,
"elbow_flex": 0.6,
"wrist_flex": 0.4,
"wrist_roll": 0.3,
"gripper": 0.2,
}
# Friction model parameters
_FRICTION_VISCOUS = { # Viscous friction coefficient [Nm⋅s/rad] per joint
"shoulder_pan": 0.02,
"shoulder_lift": 0.08,
"elbow_flex": 0.05,
"wrist_flex": 0.02,
"wrist_roll": 0.015,
"gripper": 0.01,
}
_FRICTION_COULOMB = { # Coulomb friction [Nm] per joint
"shoulder_pan": 0.05,
"shoulder_lift": 0.30,
"elbow_flex": 0.20,
"wrist_flex": 0.10,
"wrist_roll": 0.06,
"gripper": 0.04,
}
def __init__(self, config: SO101FollowerTConfig):
super().__init__(config)
@@ -112,6 +150,26 @@ class SO101FollowerT(Robot):
def is_connected(self) -> bool:
return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
@property
def kp_gains(self) -> dict[str, float]:
"""Position control gains [Nm/rad] for bilateral teleoperation"""
return self._KP_GAINS.copy()
@property
def kd_gains(self) -> dict[str, float]:
"""Velocity control gains [Nm⋅s/rad] for bilateral teleoperation"""
return self._KD_GAINS.copy()
@property
def friction_viscous(self) -> dict[str, float]:
"""Viscous friction coefficients [Nm⋅s/rad] for friction compensation"""
return self._FRICTION_VISCOUS.copy()
@property
def friction_coulomb(self) -> dict[str, float]:
"""Coulomb friction coefficients [Nm] for friction compensation"""
return self._FRICTION_COULOMB.copy()
def _current_to_torque_nm(self, raw: dict[str, Any]) -> dict[str, float]:
"""Convert "Present_Current" register counts (±2047) → torque [Nm].
Values are clamped to ±3A before conversion for protection.
@@ -178,10 +236,14 @@ class SO101FollowerT(Robot):
dq_rad: dict[str, float],
ddq_rad: dict[str, float],
tau_measured: dict[str, float],
include_friction: bool = True,
) -> dict[str, float]:
"""
Compute disturbance torques using direct model-based approach:
τ_disturbance = τ_measured - τ_gravity - τ_inertia
τ_disturbance = τ_measured - τ_gravity - τ_inertia - τ_friction
Args:
include_friction: If True, also removes friction from the disturbance calculation
"""
# Get gravity compensation
tau_gravity = self._gravity_from_q(q_rad)
@@ -193,6 +255,20 @@ class SO101FollowerT(Robot):
tau_disturbance = {}
for motor_name in self.bus.motors:
tau_dist = tau_measured[motor_name] - tau_gravity[motor_name] - tau_inertia[motor_name]
# Optionally remove friction model
if include_friction:
# Calculate friction torque using class constants
omega = dq_rad[motor_name]
tau_friction = self._FRICTION_VISCOUS[motor_name] * omega + self._FRICTION_COULOMB[
motor_name
] * (1.0 if omega > 0.01 else -1.0 if omega < -0.01 else 0.0)
# Apply torque sign correction (same as for gravity/inertia)
tau_friction = -tau_friction # Apply torque sign correction
tau_dist -= tau_friction
tau_disturbance[motor_name] = tau_dist
return tau_disturbance