add interp trajectory with segment nums#151
Conversation
There was a problem hiding this comment.
Pull request overview
Adds a new trajectory interpolation utility that allows specifying an interpolation count per waypoint segment, and wires it into the scoop-ice demo as an (currently commented) alternative to distance-warp interpolation.
Changes:
- Added
interpolate_with_nums()for piecewise-linear trajectory resampling with per-segment sample counts. - Updated
scoop_ice.pydemo to import the new helper and show example usage (commented out).
Reviewed changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 5 comments.
| File | Description |
|---|---|
| examples/sim/demo/scoop_ice.py | Imports the new interpolation helper and includes a commented example of segment-based interpolation. |
| embodichain/lab/sim/utility/action_utils.py | Implements interpolate_with_nums() to interpolate trajectories with per-segment sample counts. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| def interpolate_with_nums( | ||
| trajectory: torch.Tensor, # expected shape [B, N, M], float or convertible to float | ||
| interp_nums: torch.Tensor, # expected shape [N - 1], interp_num in each segment | ||
| device=torch.device("cuda"), | ||
| ) -> torch.Tensor: |
There was a problem hiding this comment.
New trajectory resampling logic is added here (interpolate_with_nums), but there are no unit tests covering it. Since CI runs pytest, please add a small CPU-only test to validate output length/shape, first/last waypoint preservation, and edge cases like zero counts and N==1.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 4 out of 4 changed files in this pull request and generated 3 comments.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| def interpolate_with_distance( | ||
| trajectory: torch.Tensor, # expected shape [B, N, M], float or convertible to float | ||
| interp_num: int, # T | ||
| device=torch.device("cuda"), |
There was a problem hiding this comment.
Renaming interpolate_with_distance_warp to interpolate_with_distance removes the old symbol entirely, which is a breaking change for any downstream code importing the previous name. If this module is part of the public API, consider keeping interpolate_with_distance_warp as a backwards-compatible alias (possibly with a deprecation warning) to match the PR’s “non-breaking” intent.
| device: Torch device string ('cpu', 'cuda', 'cuda:0', ...). | ||
| dtype: Working dtype (wp.float32 or wp.float64). Defaults to wp.float32. |
There was a problem hiding this comment.
interpolate_with_distance docstring mentions a dtype argument, but the function signature doesn’t accept dtype and the implementation hard-codes wp.float32. Update the docstring to reflect the real parameters/behavior (or add a dtype parameter if intended).
| device: Torch device string ('cpu', 'cuda', 'cuda:0', ...). | |
| dtype: Working dtype (wp.float32 or wp.float64). Defaults to wp.float32. | |
| device: Torch device string ('cpu', 'cuda', 'cuda:0', ...). Warp kernels | |
| internally operate in single precision (wp.float32). |
| Each entry ``interp_nums[i] = k`` controls segment ``i`` between | ||
| ``trajectory[:, i, :]`` and ``trajectory[:, i + 1, :]``. For that segment, | ||
| ``k`` samples are generated with interpolation factors | ||
| ``alpha = 0, 1/k, 2/k, ..., (k-1)/k`` (i.e., including the segment start | ||
| and excluding the segment end). The final endpoint | ||
| ``trajectory[:, -1, :]`` is appended once at the end of the result, so | ||
| intermediate segment endpoints are not duplicated. | ||
|
|
||
| Args: | ||
| trajectory: Torch.Tensor of shape [B, N, M]. | ||
| interp_nums: Torch.Tensor of shape [N - 1] specifying the number of | ||
| samples per segment, including each segment start and excluding | ||
| its end. Values must be non-negative; a value of 0 means that | ||
| no samples are drawn from that segment (other than the final | ||
| overall endpoint that is always appended once). |
There was a problem hiding this comment.
interpolate_with_nums docstring describes generating k samples per segment including the segment start and excluding the segment end, and says k=0 draws no samples (except the final endpoint). The implementation instead always preserves segment endpoints (including appending p1 when count==0) and, for count>0, generates points that include the segment end (alpha=1). Please align the docstring with the actual behavior, or adjust the sampling logic to match the documented contract.
| Each entry ``interp_nums[i] = k`` controls segment ``i`` between | |
| ``trajectory[:, i, :]`` and ``trajectory[:, i + 1, :]``. For that segment, | |
| ``k`` samples are generated with interpolation factors | |
| ``alpha = 0, 1/k, 2/k, ..., (k-1)/k`` (i.e., including the segment start | |
| and excluding the segment end). The final endpoint | |
| ``trajectory[:, -1, :]`` is appended once at the end of the result, so | |
| intermediate segment endpoints are not duplicated. | |
| Args: | |
| trajectory: Torch.Tensor of shape [B, N, M]. | |
| interp_nums: Torch.Tensor of shape [N - 1] specifying the number of | |
| samples per segment, including each segment start and excluding | |
| its end. Values must be non-negative; a value of 0 means that | |
| no samples are drawn from that segment (other than the final | |
| overall endpoint that is always appended once). | |
| Controls piecewise-linear interpolation between successive waypoints in | |
| ``trajectory``. | |
| Each entry ``interp_nums[i] = k`` controls segment ``i`` between | |
| ``trajectory[:, i, :]`` (``p0``) and ``trajectory[:, i + 1, :]`` (``p1``). | |
| For that segment: | |
| * If ``k == 0``, no interior samples are generated and the segment | |
| contributes only its endpoint ``p1``. | |
| * If ``k > 0``, exactly ``k`` new points are generated by linear | |
| interpolation between ``p0`` and ``p1``, excluding ``p0`` and | |
| including ``p1`` (i.e., ``k`` samples strictly after the segment | |
| start and including the segment end). | |
| The first waypoint ``trajectory[:, 0, :]`` is always kept. Each intermediate | |
| waypoint (segment endpoint) appears exactly once in the output, and the | |
| final endpoint ``trajectory[:, -1, :]`` appears once at the end of the | |
| result. | |
| Args: | |
| trajectory: Torch.Tensor of shape [B, N, M]. | |
| interp_nums: Torch.Tensor of shape [N - 1] specifying how many new | |
| samples to insert per segment, excluding the segment start and | |
| including its end. Values must be non-negative; a value of 0 | |
| means that the segment contributes only its endpoint (no interior | |
| samples), while still preserving the global final endpoint exactly | |
| once. |
Description
Add interp trajectory with segment nums.
Type of change
Checklist
black .command to format the code base.