feat: add Coulomb friction feedforward compensation with YAML-driven config

[JayKuluky]

Motivation

Direct-drive and low-ratio robotic arms (like YAM Ultra) exhibit noticeable static friction (stiction) on the proximal joints (J0–J2). When teleoperating in leader/follower mode, small leader motions often fail to break free of stiction on the follower without raising the position-loop kp gain — which in turn degrades compliance and makes the system feel stiff.

The standard fix is Coulomb friction feedforward (also called "Cff" in MIT mode controllers): a small velocity-direction-dependent torque added on top of the position-loop output, sized just enough to overcome each joint's stiction. This lets kp stay low (compliant) while still making the follower track small leader motions accurately.

This PR adds that capability to motor_chain_robot and exposes it through the existing arm YAML configs so per-arm tuned values can live in the repo alongside the rest of the kinematic/dynamic config.

Design

Friction compensation is applied at the controller layer, on top of whatever torque the existing control mode (position, MIT, etc.) computes. For each joint i with measured velocity q̇ᵢ:

τ_ff,i = breakaway_i · tanh(q̇ᵢ / eps)

• breakaway_i (Nm, per joint) — the magnitude of the feedforward, tuned to approximately match the joint's static-friction torque. Set to 0 for any joint that does not need comp.
• eps (rad/s) — the saturation width of the smooth sign function. Smaller values approach an ideal Coulomb sign; larger values smooth the transition through zero velocity (less chatter when the joint is nearly stationary).

tanh is chosen over a hard sign() to avoid torque chatter at zero velocity, which can excite the actuator at the control rate.

The compensation is off by default (enable_friction_comp=False and an empty breakaway tuple both disable it), so existing users see no behavior change.

API changes

motor_chain_robot constructor / get_yam_robot() factory

Three new parameters:

Parameter	Type	Default	Description
enable_friction_comp	bool	False	Master switch. When False, no comp is applied regardless of breakaway.
friction_comp_breakaway	np.ndarray | None	None	Per-joint breakaway torque (Nm). Length must equal the chain's DOF count. None or all-zeros disables comp.
friction_comp_eps	float	0.01	Saturation width (rad/s) for the smooth tanh.

YAML config (per-arm, e.g. yam_ultra.yml)

New optional friction_comp block:

friction_comp:
  enabled: true
  breakaway: [1.2, 1.8, 1.6, 0.1, 0, 0, 0]   # length must match DOF
  eps: 0.005

When the YAML provides values, they are loaded as defaults at robot construction time. Constructor kwargs (or downstream CLI flags) take precedence over YAML values when explicitly passed. This lets users commit a known-good tuning per arm while still allowing temporary overrides during tuning sessions.

If the friction_comp block is absent, behavior is identical to today.

Behavior

• No comp (default): τ_ff = 0 for all joints. Bit-identical control output to the pre-PR behavior.
• Enabled, breakaway > 0 on joint i: τ_ff,i = breakaway_i · tanh(q̇ᵢ / eps) is added to the joint torque command.
• Enabled, breakaway = 0 on joint i: that joint receives no comp (useful for distal joints that don't suffer stiction, e.g. wrist/gripper).
• At rest (|q̇| ≪ eps): tanh(q̇/eps) ≈ q̇/eps, so τ_ff is small and proportional to velocity — no jitter at zero crossings.

Backward compatibility

• All new parameters default to disabled. Existing callers that don't pass them see no change.
• YAML configs without the new friction_comp block load identically to today.
• No public type signatures are changed (only additive kwargs).

Testing

• Unit tests for the comp math:
  • breakaway = 0 → zero output for any velocity
  • Sign flips correctly across q̇ = 0
  • Saturation: |q̇| ≫ eps → output saturates at ±breakaway
  • Smoothness: derivative is continuous through zero (no sign() chatter)
• YAML round-trip: a config with friction_comp loads with the expected values; a config without it loads with comp disabled
• Constructor-vs-YAML precedence: explicit kwargs override YAML values
• Bench validation on a YAM arm:
  • With comp off, log the position error during slow leader motions on J0–J2
  • With comp on (tuned breakaway), log again — error should drop noticeably on the proximal joints without affecting distal joints

Future work / open questions

• Velocity source: currently uses the controller's measured/filtered velocity. Could later support an estimated velocity from position differencing with a configurable filter, for joints whose velocity sensor is noisy.
• Direction-asymmetric breakaway: some joints have different stiction in the two directions of motion (gravity-loaded). A breakaway_pos / breakaway_neg split could be added later if needed; current per-joint scalar is a deliberate first step to keep the API small.
• Adaptive identification: out of scope here, but the breakaway values are good candidates for offline identification from a chirp test. Happy to discuss in a follow-up.

Commits in this PR

1. feat(motor_chain_robot): add friction compensation parameters and methods — adds enable_friction_comp, friction_comp_breakaway, friction_comp_eps to the robot constructor; implements the tanh Cff computation; integrates it into the existing per-cycle torque pipeline.
2. feat(yam_config): load friction comp params from YAML — extends the YAM config loader to read friction_comp: blocks from the per-arm YAML files; constructor kwargs continue to take precedence.

Notes for reviewers

This work was developed against a downstream consumer (a teleoperation pipeline) where the immediate use case is bi-manual YAM follower stiction on small leader motions. The downstream side simply forwards CLI flags into the same three constructor kwargs introduced here — no specialized integration glue is required on the SDK side.

Happy to split commits, rename parameters, adjust the YAML schema, or scope the PR down (e.g. ship the params/methods first, YAML loader as a follow-up) based on review preference.