Skip to content

add interp trajectory with segment nums #151

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
matafela merged 3 commits into from
Feb 28, 2026
Merged

add interp trajectory with segment nums #151

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2bc4bd5
update
Feb 27, 2026
dce6012
rename
Feb 27, 2026
e78f893
update
Feb 27, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
76 changes: 74 additions & 2 deletions embodichain/lab/sim/utility/action_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -245,7 +245,7 @@ def get_trajectory_object_offset_qpos(
return is_success, key_qpos_offset


def interpolate_with_distance_warp(
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"),
Comment on lines +248 to 251
Copy link

Copilot AI Feb 27, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copilot uses AI. Check for mistakes.
Expand All @@ -258,7 +258,7 @@ def interpolate_with_distance_warp(
Args:
trajectory: Torch.Tensor of shape [B, N, M].
interp_num: Target number of samples T.
device: Warp device string ('cpu', 'cuda', 'cuda:0', ...).
device: Torch device string ('cpu', 'cuda', 'cuda:0', ...).
dtype: Working dtype (wp.float32 or wp.float64). Defaults to wp.float32.
Comment on lines +261 to 262
Copy link

Copilot AI Feb 27, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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).

Suggested change
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).

Copilot uses AI. Check for mistakes.

Returns:
Expand Down Expand Up @@ -335,3 +335,75 @@ def interpolate_with_distance_warp(
# wp.synchronize_device(device)
interp_trajectory = wp.to_torch(out).view(B, T, M)
return interp_trajectory


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:
Comment on lines +340 to +344
Copy link

Copilot AI Feb 27, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copilot uses AI. Check for mistakes.
"""
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).
Comment on lines +346 to +360
Copy link

Copilot AI Feb 27, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Suggested change
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.

Copilot uses AI. Check for mistakes.
device: Torch device string ('cpu', 'cuda', 'cuda:0', ...).

Returns:
Torch.Tensor of interpolated trajectories.
"""
trajectory = trajectory.to(device)
if not torch.is_floating_point(trajectory):
trajectory = trajectory.float()

B, N, M = trajectory.shape
if N == 0:
return trajectory.new_empty((B, 0, M))

interp_nums_tensor = torch.as_tensor(interp_nums, device="cpu").reshape(-1)
if interp_nums_tensor.numel() != max(N - 1, 0):
raise ValueError("`interp_nums` must have shape (N - 1,).")

if N == 1:
return trajectory[:, :1, :]

interp_nums_list = interp_nums_tensor.to(torch.int64).tolist()

# Always seed the output with the first waypoint so it is never dropped,
# even when leading segments have zero samples.
segments = [trajectory[:, :1, :]]
for i, count in enumerate(interp_nums_list):
if count < 0:
raise ValueError("`interp_nums` values must be non-negative.")
p0 = trajectory[:, i : i + 1, :]
p1 = trajectory[:, i + 1 : i + 2, :]
if count == 0:
# No interpolated samples for this segment, but ensure the endpoint
# waypoint is still present so zero-sample segments don't remove it.
segments.append(p1)
continue
# Generate linearly spaced interpolation parameters from 0 to 1
# (inclusive), then drop the first value (t = 0) because p0 is
# already the last point in `segments`. This appends exactly
# `count` new points per segment and preserves all endpoints.
alpha = torch.linspace(
0.0,
1.0,
steps=count + 1,
device=device,
dtype=trajectory.dtype,
).view(1, count + 1, 1)
seg = p0 + (p1 - p0) * alpha
segments.append(seg[:, 1:, :])
return torch.cat(segments, dim=1)
4 changes: 2 additions & 2 deletions examples/sim/demo/grasp_cup_to_caffe.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
RigidBodyAttributesCfg,
ArticulationCfg,
)
from embodichain.lab.sim.utility.action_utils import interpolate_with_distance_warp
from embodichain.lab.sim.utility.action_utils import interpolate_with_distance
from embodichain.lab.sim.shapes import MeshCfg
from embodichain.data import get_data_path
from embodichain.utils import logger
Expand Down Expand Up @@ -374,7 +374,7 @@ def create_trajectory(
)
all_trajectory = torch.cat([arm_trajectory, hand_trajectory], dim=-1)
# trajetory with shape [n_envs, n_waypoint, dof]
interp_trajectory = interpolate_with_distance_warp(
interp_trajectory = interpolate_with_distance(
trajectory=all_trajectory, interp_num=150, device=sim.device
)
return interp_trajectory
Expand Down
4 changes: 2 additions & 2 deletions examples/sim/demo/press_softbody.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@

from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
from embodichain.lab.sim.objects import Robot, SoftObject
from embodichain.lab.sim.utility.action_utils import interpolate_with_distance_warp
from embodichain.lab.sim.utility.action_utils import interpolate_with_distance
from embodichain.lab.sim.shapes import MeshCfg
from embodichain.lab.sim.solvers import PytorchSolverCfg
from embodichain.data import get_data_path
Expand Down Expand Up @@ -181,7 +181,7 @@ def press_cow(sim: SimulationManager, robot: Robot):
)

arm_trajectory = torch.concatenate([arm_start_qpos, approach_qpos])
interp_trajectory = interpolate_with_distance_warp(
interp_trajectory = interpolate_with_distance(
trajectory=arm_trajectory[None, :, :], interp_num=50, device=sim.device
)
interp_trajectory = interp_trajectory[0]
Expand Down
4 changes: 2 additions & 2 deletions examples/sim/demo/scoop_ice.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
LightCfg,
)
from embodichain.lab.sim.material import VisualMaterialCfg
from embodichain.lab.sim.utility.action_utils import interpolate_with_distance_warp
from embodichain.lab.sim.utility.action_utils import interpolate_with_distance
from embodichain.lab.sim.shapes import MeshCfg, CubeCfg
from embodichain.lab.sim.solvers import PytorchSolverCfg
from embodichain.data import get_data_path
Expand Down Expand Up @@ -515,7 +515,7 @@ def scoop_ice(sim: SimulationManager, robot: Robot, scoop: RigidObject):
)

all_trajectory = torch.hstack([arm_trajectory, hand_trajectory])
interp_trajectory = interpolate_with_distance_warp(
interp_trajectory = interpolate_with_distance(
trajectory=all_trajectory[None, :, :], interp_num=200, device=sim.device
)
interp_trajectory = interp_trajectory[0]
Expand Down