Piper Trajectory Torque Workflow

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.