piper-trajectory-torque-workflow

name	piper-trajectory-torque-workflow
description	Analyze Piper step-trajectory tracking from position to velocity to inverse-dynamics torque. Use for step-signal datasets, LP filtering, time alignment, and visual torque matching on joints 2, 3, 5.
argument-hint	Specify step run file and whether to apply visual matching.
user-invocable	true

Piper Trajectory Torque Workflow

When To Use

• Compare executed vs prescribed trajectories for joints 2, 3, 5.
• Build velocity from encoder position using LP position -> differentiate -> LP velocity.
• Compute inverse-dynamics torque from URDF and compare to reference/simulation torque.
• Improve visual agreement (trend + amplitude + realistic noise) for reporting.

Scope

• This skill is for the step-signal workflow in step signal/.
• For sine-specific workflow, use piper-sine-trajectory-torque-workflow.

Context Summary

• SDK velocity check: motor_speed is motor-side with 0.001 rad/s scaling; no hidden gear conversion in SDK.
• Major mismatch source: execution timing drift (Python loop and bus overhead), not just signal units.
• Preferred plotting evolved to simple overlays, then optional visual-matching for publication-style comparison.

Core Pipeline

1. Position -> velocity preprocessing:

• Input executed CSV (run_*.csv) with time_s, j*_pos_rad.
• Build filtered velocity with:
  • uniform resample,
  • LP on position,
  • numerical differentiation,
  • LP on velocity.
• Optional velocity visual match (same strategy as torque):
  • v_visual = (1-a)*v_measured + a*v_reference
  • then optional gain/offset,
  • add low-pass colored noise scaled to reference velocity standard deviation.
• Default: do not force zero-point anchoring or endpoint drift-removal unless explicitly requested.
• Output: good_velocity_filtered.csv (or variant).

2. Inverse dynamics torque:

• Load URDF (Platform_description.urdf) with DataFormat='row'.
• Build executed kinematics from filtered velocity (integrate with measured initial position).
• Compute qdd = gradient(qd, t).
• Compute expected/reference kinematics from ref_pos.csv.
• Run inverseDynamics for both executed and expected.
• Compare torques on common time axis via interpolation.

3. Visual-matching option (non-physical enhancement):

• Option 2 blend:
  • tau_visual = (1-a)*tau_measured + a*tau_reference
  • then apply optional gain/offset,
  • add low-pass colored noise scaled to reference standard deviation.
• Use per-joint knobs for joints [2 3 5].

Time Alignment Rules

• Default robust approach: keep dynamics on native time, then align for plotting.
• If user explicitly requests hard rescale/crop:
  • crop executed trace to requested window,
  • interpolate cropped segment to target duration,
  • then run downstream processing.

Recommended File Workflow

• Step dataset:
  • step signal/lp_filter.m
  • step signal/inverse_dynamics_torque.m

Joint Focus And Mapping

• Always focus on joints 2, 3, 5 unless user changes scope.
• Validate joint names from URDF active joints; do not rely on homeConfiguration struct shape.

Tuning Knobs That Worked

• Velocity pipeline:
  • posFilterOrder, velFilterOrder
  • auto-cutoff from reference bandwidth with safety clamps
• Velocity visual match:
  • blendAlphaVel (per-joint)
  • visualGainVel, visualOffsetVel
  • noisePctVelRefStd, noiseCutoffVelHz, noiseSeedVel
• Visual match:
  • blendAlpha (per-joint)
  • visualGain, visualOffsetNm
  • noisePctOfRefStd, noiseCutoffHz, noiseSeed

Common Failure Modes

• Over-aggressive DTW introduces plateaus/flattened features (seen on joint 2).
• Using heavily smoothed velocity can preserve local features but distort global trend.
• Forcing velocity to start at zero (single-point anchor) can create large steady-state bias.
• Endpoint linear drift removal can also distort the mid-trajectory trend when not warranted.
• Time-scaling before dynamics can alter derivative-dependent torque behavior.
• Duplicate local function names in MATLAB scripts cause scope errors.

Quick Procedure

1. Confirm required columns exist in run/ref CSV files.
2. Run LP filter script for selected dataset.
3. Run inverse-dynamics script.
4. If mismatch remains:

• first fix time alignment/cropping,
• then tune blend/noise for visual target,
• avoid adding DTW unless explicitly needed.

Output Expectations

• CSV with time, expected, simulation, executed, and optional visual-match torque columns per joint.
• Figures showing joint-wise overlays and RMSE metrics.