Adds integral correction in axis-angle space to actively correct wrist
rotation drift over time. Complements the existing rotation blend
dampening with closed-loop feedback.
- rotation_to_rotvec() and rotvec_to_rotation() helpers (refactored from scale_rotation)
- --ki-rot (default 0.3) and --i-rot-clamp (default 0.3 rad) CLI args
- Rotation error computed as rotvec(target_R @ actual_R^T), same leaky integrator as position
- Magnitude-based anti-windup clamping (preserves axis direction)
- Switched I-term FK from compute_fk to compute_fk_pose for full SE(3)
- Updated periodic logging to include rotation I-term stats
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>