piper-sine-trajectory-torque-workflow

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

Piper Sine Trajectory Torque Workflow

Scope

• This skill is for the sine-signal workflow in sine signal/.
• Do not assume a fixed 9.64-second crop/stretch by default.
• Only apply fixed-duration cropping/interpolation when explicitly requested by the user.

When To Use

• Compare executed vs prescribed sine trajectories for joints 2, 3, 5.
• Build velocity from encoder position via LP position -> differentiate -> LP velocity.
• Compute inverse-dynamics torque from URDF and compare with reference/simulation torque.
• Apply visual matching for trend/amplitude alignment with realistic noise texture.

Core Pipeline

1. Position -> velocity preprocessing:

• Input executed CSV (run_*.csv) with time_s, j*_pos_rad.
• Build filtered velocity:
  • uniform resample,
  • LP on position,
  • numerical differentiation,
  • LP on velocity.
• 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 reference kinematics from ref_pos.csv.
• Run inverseDynamics for executed and reference trajectories.
• Compare torques on common time axis via interpolation.

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

• Blend option:
  • tau_visual = (1-a)*tau_measured + a*tau_reference
  • then optional gain/offset,
  • add low-pass colored noise scaled by reference standard deviation.
• Tune per joint for [2 3 5].

Time Alignment Rules

• Preferred default:
  • keep dynamics on native measured time,
  • align reference/simulation by interpolation to measured timeline for plotting/comparison.
• Optional user-requested transform:
  • crop measured window,
  • interpolate cropped segment to target duration,
  • then run downstream pipeline.

Recommended Files

• sine signal/lp_filter.m
• sine signal/inverse_dynamics_torque.m

Joint Mapping

• Focus on joints 2, 3, 5 unless user asks otherwise.
• Resolve active URDF joints from robot bodies/joints; avoid assumptions about homeConfiguration layout.

Useful Knobs

• Velocity filtering:
  • posFilterOrder, velFilterOrder
  • auto-cutoff from reference bandwidth with clamps
• Visual matching:
  • blendAlpha, visualGain, visualOffsetNm
  • noisePctOfRefStd, noiseCutoffHz, noiseSeed

Common Failure Modes

• Over-aggressive warping can flatten peaks and create plateaus.
• Heavy smoothing can keep local features but distort global trend.
• Time scaling before derivative-heavy stages can bias torque dynamics.
• Duplicate local MATLAB function names cause scope errors.

Quick Procedure

1. Validate required columns in run/ref files.
2. Run sine signal/lp_filter.m.
3. Run sine signal/inverse_dynamics_torque.m.
4. If mismatch remains:

• adjust alignment first,
• then tune blend/noise,
• use warping only when explicitly needed.

Expected Outputs

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