"""
Low-level trajectory manipulation for scene edition.
The :class:`TrajectoryEditor` provides comprehensive trajectory manipulation
capabilities and is used internally by :class:`EgoEditor` and :class:`ActorEditor`.
It can also be accessed directly for advanced use cases requiring fine-grained
control over trajectory modifications.
Example
-------
>>> from mimic_scene_edition.editors.trajectory_editor import TrajectoryEditor
>>> te = TrajectoryEditor(original_traj)
>>> te.translate(forward=5)
>>> te.rotate(yaw=15)
>>> modified_traj = te.traj # Access the result
"""
from __future__ import annotations
from typing import Optional, Literal
import numpy as np
from mimic_data_api.trajectory_numpy import WorldTrajectory
from mimic_scene_edition.editors.base import (
log_method,
check_value_limits,
validate_referential,
)
from mimic_scene_edition.core.geometry import (
extract_translation_vector,
euler_xyz_deg,
make_T,
)
from mimic_scene_edition.core import trajectory
from mimic_scene_edition.core.interpolate import interpolate_transforms_at_times
from mimic_scene_edition.core.velocity import estimate_velocities_world
from mimic_scene_edition.strategies.curves import build_lane_change_T_seq
Referential = Literal["local", "world"]
"""Coordinate frame for transformations: ``"local"`` or ``"world"``."""
Axis = Literal["x", "y", "z"]
"""Rotation/motion axis: ``"x"`` (forward), ``"y"`` (left), or ``"z"`` (up)."""
[docs]
class TrajectoryEditor:
"""Low-level trajectory manipulation editor.
Provides comprehensive methods for editing :class:`WorldTrajectory` objects,
including SE(3) transformations, temporal operations, motion strategies,
and utility functions.
Parameters
----------
traj : WorldTrajectory
The trajectory to edit. This object is modified in place.
recompute_on_mutation : bool, optional
If True (default), automatically recompute velocities after
trajectory modifications that affect motion.
limited_mode : bool, optional
If True, enable parameter value limit checking. Default is False.
value_limits : dict[str, tuple[float, float]] | None, optional
Dictionary mapping parameter names to (min, max) bounds.
Used when ``limited_mode`` is True.
editions : list[dict] | None, optional
List to append edition records to. If None, creates a new list.
Attributes
----------
traj : WorldTrajectory
The trajectory being edited.
recompute_on_mutation : bool
Whether to auto-recompute velocities after modifications.
limited_mode : bool
Whether parameter limit checking is enabled.
value_limits : dict
Parameter value limits for validation.
Notes
-----
The TrajectoryEditor uses a fluent interface pattern - most methods
return ``self`` to enable method chaining.
After editing, access the modified trajectory via the :attr:`traj` attribute.
Examples
--------
Basic transformation:
>>> te = TrajectoryEditor(original_traj)
>>> te.translate(forward=5).rotate(yaw=15)
>>> modified = te.traj
Complex manipulation with chaining:
>>> te = TrajectoryEditor(traj)
>>> te.displace(forward=10) \\
... .lane_change(start_idx=20, duration=30, lateral=3.5) \\
... .freeze_from(100)
"""
[docs]
def __init__(
self,
traj: WorldTrajectory,
*,
recompute_on_mutation: bool = True,
limited_mode: bool = False,
value_limits: dict[str, tuple[float, float]] | None = None,
editions: list[dict] | None = None,
):
self.traj = traj
self.recompute_on_mutation = recompute_on_mutation
self.limited_mode = limited_mode
self.value_limits = value_limits or {}
self._editions = editions if editions is not None else []
# ---------- internals ----------
def _set(
self, new_traj: WorldTrajectory, *, needs_vel_update: bool
) -> "TrajectoryEditor":
"""Set the trajectory and optionally recompute velocities.
Parameters
----------
new_traj : WorldTrajectory
The new trajectory to set.
needs_vel_update : bool
If True and ``recompute_on_mutation`` is True, recompute
linear and angular velocities from the pose sequence.
Returns
-------
TrajectoryEditor
Self for method chaining.
"""
self.traj = new_traj
if self.recompute_on_mutation and needs_vel_update:
lin, ang = estimate_velocities_world(self.traj)
self.traj.linear_velocity = lin
self.traj.angular_velocity = ang
return self
@staticmethod
def _window(
N: int, start_idx: Optional[int], end_idx: Optional[int]
) -> tuple[int, int]:
"""Validate and normalize frame index window bounds.
Parameters
----------
N : int
Total number of frames in the trajectory.
start_idx : int | None
Start index (None means 0).
end_idx : int | None
End index exclusive (None means N).
Returns
-------
tuple[int, int]
Validated (start, end) indices.
Raises
------
ValueError
If the window bounds are invalid.
"""
start = 0 if start_idx is None else int(start_idx)
end = N if end_idx is None else int(end_idx)
if not (0 <= start <= end <= N):
raise ValueError(f"Invalid window [{start}, {end}) for length {N}.")
return start, end
# ---------- SE(3) ----------
[docs]
@check_value_limits
@log_method
@validate_referential("referential")
def translate(
self, referential: Referential = "world", **xyz: float
) -> "TrajectoryEditor":
"""Translate the trajectory using semantic direction components.
Applies a translation to all poses in the trajectory using
intuitive direction names instead of raw XYZ values.
Parameters
----------
referential : {"local", "world"}, optional
Coordinate frame for the translation:
- ``"local"``: Translation in the object's local frame
(x=forward, y=left, z=up). Applied as post-multiplication.
- ``"world"``: Translation in world coordinates.
Applied as pre-multiplication.
Default is ``"world"``.
**xyz : float
Named translation components:
- ``forward``: Distance forward (positive X) in meters
- ``backward``: Distance backward (negative X) in meters
- ``left``: Distance left (positive Y) in meters
- ``right``: Distance right (negative Y) in meters
- ``up``: Distance up (positive Z) in meters
- ``down``: Distance down (negative Z) in meters
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.translate(forward=5, left=2) # World frame
>>> te.translate(forward=3, referential="local") # Local frame
See Also
--------
transform : Combined translation and rotation.
"""
t = extract_translation_vector(**xyz)
if not np.any(t):
return self
if referential == "local":
nt = trajectory.translate_local(self.traj, t)
return self._set(nt, needs_vel_update=True)
else:
# world: build T with only translation and pre-multiply via core
T = make_T(np.eye(3), t)
nt, needs = trajectory.apply_T_sequence(self.traj, T, referential="world")
return self._set(nt, needs_vel_update=needs)
[docs]
@check_value_limits
@log_method
@validate_referential("referential")
def rotate(
self,
*,
roll: float = 0.0,
pitch: float = 0.0,
yaw: float = 0.0,
referential: Referential = "world",
start_idx: Optional[int] = None,
end_idx: Optional[int] = None,
) -> "TrajectoryEditor":
"""Rotate the trajectory using Euler angles.
Applies a rotation to poses in the specified frame range
using roll-pitch-yaw Euler angles in degrees.
Parameters
----------
roll : float, optional
Rotation around the X (forward) axis in degrees. Default is 0.
pitch : float, optional
Rotation around the Y (left) axis in degrees. Default is 0.
yaw : float, optional
Rotation around the Z (up) axis in degrees. Default is 0.
referential : {"local", "world"}, optional
Coordinate frame for the rotation:
- ``"local"``: Rotation in object's local frame (post-multiply).
- ``"world"``: Rotation in world frame (pre-multiply).
Default is ``"world"``.
start_idx : int | None, optional
First frame to rotate (inclusive). None means 0.
end_idx : int | None, optional
Last frame to rotate (exclusive). None means end.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.rotate(yaw=90) # Turn 90° left
>>> te.rotate(pitch=5, start_idx=10, end_idx=50) # Pitch up in range
See Also
--------
transform : Combined translation and rotation.
"""
if not (roll or pitch or yaw):
return self
if referential == "local" and start_idx is None and end_idx is None:
nt = trajectory.rotate_local_euler(
self.traj, roll=roll, pitch=pitch, yaw=yaw
)
# constant local rotation preserves relative motion: no recompute needed
return self._set(nt, needs_vel_update=False)
# generic path with window and world referential
Rm = euler_xyz_deg(roll, pitch, yaw)
T = make_T(Rm, np.zeros(3))
return self.apply_T_sequence(
T, start_idx=start_idx, end_idx=end_idx, referential=referential
)
[docs]
@log_method
@validate_referential("referential")
def apply_T_sequence(
self,
T_seq: np.ndarray,
*,
start_idx: Optional[int] = None,
end_idx: Optional[int] = None,
referential: Referential = "world",
) -> "TrajectoryEditor":
"""Apply a sequence of 4x4 transformation matrices.
Applies SE(3) transformation matrices to poses in the
specified frame range. Can apply a single matrix to all
frames or a sequence of per-frame matrices.
Parameters
----------
T_seq : np.ndarray
Transformation matrices with shape ``(4, 4)`` for a single
transform applied to all frames, or ``(N, 4, 4)`` for
per-frame transforms where N matches the frame range size.
start_idx : int | None, optional
First frame to transform (inclusive). None means 0.
end_idx : int | None, optional
Last frame to transform (exclusive). None means end.
referential : {"local", "world"}, optional
How to apply the transformation:
- ``"world"``: Left-multiply (T @ pose).
- ``"local"``: Right-multiply (pose @ T).
Default is ``"world"``.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> # Apply single transform to all frames
>>> te.apply_T_sequence(T_4x4, referential="world")
>>> # Apply per-frame transforms
>>> T_seq = np.zeros((100, 4, 4))
>>> # ... fill T_seq ...
>>> te.apply_T_sequence(T_seq, start_idx=10, end_idx=110)
"""
nt, needs = trajectory.apply_T_sequence(
self.traj,
T_seq,
start_idx=start_idx,
end_idx=end_idx,
referential=referential,
)
return self._set(nt, needs_vel_update=needs)
# ---------- Temporal ----------
[docs]
@check_value_limits
@log_method
def resample_to_rate(self, hz: float) -> "TrajectoryEditor":
"""Resample the trajectory to a target frame rate.
Interpolates the trajectory to produce poses at a uniform
temporal spacing determined by the specified frequency.
Parameters
----------
hz : float
Target frame rate in Hz (frames per second).
Must be positive.
Returns
-------
TrajectoryEditor
Self for method chaining.
Raises
------
ValueError
If hz is not positive.
Examples
--------
>>> te.resample_to_rate(10.0) # Resample to 10 Hz
>>> te.resample_to_rate(30.0) # Resample to 30 Hz
See Also
--------
resample_to_times : Resample to specific timestamps.
"""
if hz <= 0:
raise ValueError("hz must be > 0")
t0, tN = float(self.traj.timestamps[0]), float(self.traj.timestamps[-1])
if tN <= t0:
return self
n = max(2, int(round((tN - t0) * hz)) + 1)
query_t = t0 + np.arange(n) / hz
return self.resample_to_times(query_t)
[docs]
@check_value_limits
@log_method
def resample_to_times(self, new_t: np.ndarray) -> "TrajectoryEditor":
"""Resample the trajectory to specific timestamps.
Interpolates poses to the specified query timestamps using
SE(3) interpolation for smooth results.
Parameters
----------
new_t : np.ndarray
Array of target timestamps to sample at.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> new_times = np.linspace(0, 10, 100)
>>> te.resample_to_times(new_times)
See Also
--------
resample_to_rate : Resample to a uniform frame rate.
"""
query_t = np.asarray(new_t, float)
new_T = interpolate_transforms_at_times(self.traj, query_t)
# One construction, not two assignments — see the note in :meth:`slice`.
nt = WorldTrajectory(timestamps=query_t, object2world=new_T)
return self._set(nt, needs_vel_update=True)
[docs]
@check_value_limits
@log_method
def offset_frames(self, k: int) -> "TrajectoryEditor":
"""Circular shift the trajectory by k frames.
Shifts all poses and timestamps by k frames in a circular
manner, wrapping around at the boundaries.
Parameters
----------
k : int
Number of frames to shift. Positive values shift forward
in time, negative values shift backward.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.offset_frames(5) # Shift forward 5 frames
>>> te.offset_frames(-3) # Shift backward 3 frames
"""
nt = trajectory.offset_frames(self.traj, int(k))
return self._set(nt, needs_vel_update=False)
[docs]
@check_value_limits
@log_method
def slice(self, start_idx: int, end_idx: int) -> "TrajectoryEditor":
"""Keep only frames in the specified range.
Truncates the trajectory to contain only poses within
the specified index range.
Parameters
----------
start_idx : int
First frame to keep (inclusive).
end_idx : int
Last frame to keep (exclusive).
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.slice(10, 50) # Keep frames 10-49
"""
Tw = np.asarray(self.traj.object2world, float)
t = np.asarray(self.traj.timestamps, float)
i0, i1 = self._window(len(t), start_idx, end_idx)
# Build the new trajectory in one go. ``WorldTrajectory`` validates on
# assignment, so setting ``timestamps`` first would compare the new
# length against the old ``object2world`` and raise. The velocity arrays
# are left to ``_set``, which recomputes them at the new length.
nt = WorldTrajectory(timestamps=t[i0:i1], object2world=Tw[i0:i1])
return self._set(nt, needs_vel_update=True)
# ---------- Strategies / maneuvers ----------
[docs]
@check_value_limits
@log_method
def freeze(
self, start_idx: int, end_idx: int | None = None, *, persist_tail: bool = True
) -> "TrajectoryEditor":
"""Freeze poses in a range to a static position.
Makes the object stationary by repeating the pose at ``start_idx``
for the specified range. Velocities are zeroed in the frozen region.
Parameters
----------
start_idx : int
Index of the frame to freeze at.
end_idx : int | None, optional
End of freeze range (exclusive). If None, freeze to
the end of the trajectory.
persist_tail : bool, optional
If True (default) and ``end_idx`` is specified, maintain
continuity with poses after the frozen region.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.freeze(50) # Freeze from frame 50 to end
>>> te.freeze(30, 60) # Freeze frames 30-59 only
See Also
--------
freeze_from : Shortcut for freezing to end.
freeze_window : Freeze a specific window.
"""
nt = trajectory.freeze(
self.traj,
int(start_idx),
None if end_idx is None else int(end_idx),
persist_tail=persist_tail,
)
self._set(nt, needs_vel_update=True)
i0 = int(start_idx)
if end_idx is None:
if self.traj.linear_velocity is not None:
self.traj.linear_velocity[i0:] = 0.0
if self.traj.angular_velocity is not None:
self.traj.angular_velocity[i0:] = 0.0
else:
i1 = int(end_idx)
if self.traj.linear_velocity is not None:
self.traj.linear_velocity[i0:i1] = 0.0
if self.traj.angular_velocity is not None:
self.traj.angular_velocity[i0:i1] = 0.0
return self
[docs]
@check_value_limits
@log_method
def freeze_from(self, idx: int) -> "TrajectoryEditor":
"""Freeze the trajectory from an index to the end.
Convenience method equivalent to ``freeze(idx, None)``.
Parameters
----------
idx : int
Index to start freezing from.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.freeze_from(50) # Freeze from frame 50 onwards
See Also
--------
freeze : Full freeze functionality.
"""
return self.freeze(idx, None)
[docs]
@check_value_limits
@log_method
def freeze_window(
self, start_idx: int, end_idx: int, *, persist_tail: bool = True
) -> "TrajectoryEditor":
"""Freeze a specific window of frames.
Convenience method equivalent to ``freeze(start_idx, end_idx)``.
Parameters
----------
start_idx : int
First frame to freeze (inclusive).
end_idx : int
Last frame to freeze (exclusive).
persist_tail : bool, optional
If True (default), maintain continuity after the frozen region.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.freeze_window(30, 60) # Freeze frames 30-59
See Also
--------
freeze : Full freeze functionality.
"""
return self.freeze(start_idx, end_idx, persist_tail=persist_tail)
[docs]
@log_method
def spin(
self,
*,
period_s: Optional[float] = None,
rpm: Optional[float] = None,
axis: Axis = "z",
start_idx: Optional[int] = None,
end_idx: Optional[int] = None,
) -> "TrajectoryEditor":
"""Apply continuous rotation (spinning) to the trajectory.
Makes the object rotate continuously around the specified axis
at a constant rate defined by either period or RPM.
Parameters
----------
period_s : float | None, optional
Seconds per full revolution. Mutually exclusive with ``rpm``.
rpm : float | None, optional
Revolutions per minute. Mutually exclusive with ``period_s``.
axis : {"x", "y", "z"}, optional
Rotation axis. Default is ``"z"`` (vertical).
start_idx : int | None, optional
First frame to apply spin (inclusive). None means 0.
end_idx : int | None, optional
Last frame to apply spin (exclusive). None means end.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.spin(rpm=60, axis="z") # 60 RPM around vertical axis
>>> te.spin(period_s=2.0) # One revolution per 2 seconds
"""
nt = trajectory.spin(
self.traj,
period_s=period_s,
rpm=rpm,
axis=axis,
start_idx=start_idx,
end_idx=end_idx,
)
return self._set(nt, needs_vel_update=True)
[docs]
@check_value_limits
@log_method
def displace_along_forward(
self,
meters: float,
*,
start_idx: Optional[int] = None,
end_idx: Optional[int] = None,
) -> "TrajectoryEditor":
"""Displace the trajectory along its path by arc-length.
Moves poses along the trajectory path (not in a straight line)
by the specified distance.
Parameters
----------
meters : float
Distance to displace in meters. Positive moves forward,
negative moves backward along the path.
start_idx : int | None, optional
First frame to displace (inclusive). None means 0.
end_idx : int | None, optional
Last frame to displace (exclusive). None means end.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.displace_along_forward(10.0) # Move 10m forward on path
>>> te.displace_along_forward(-5.0) # Move 5m backward on path
See Also
--------
displace : Displacement with named direction parameters.
"""
return self.displace(
forward=float(meters), start_idx=start_idx, end_idx=end_idx
)
[docs]
@check_value_limits
@log_method
def displace(
self,
*,
forward: float = 0.0,
backward: float = 0.0,
start_idx: Optional[int] = None,
end_idx: Optional[int] = None,
) -> "TrajectoryEditor":
"""Displace poses along the trajectory path by arc-length.
Unlike :meth:`translate`, this moves the object along the path
it's already following, not in a straight line. This is useful
for positioning actors relative to their motion path.
Parameters
----------
forward : float, optional
Distance to move forward along path in meters. Default is 0.
backward : float, optional
Distance to move backward along path in meters. Default is 0.
start_idx : int | None, optional
First frame to displace (inclusive). None means 0.
end_idx : int | None, optional
Last frame to displace (exclusive). None means end.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.displace(forward=10) # Place 10m ahead on path
>>> te.displace(backward=5) # Place 5m behind on path
Notes
-----
For lateral offset, chain with :meth:`transform`:
>>> te.displace(forward=10).transform(left=2)
See Also
--------
displace_along_forward : Shortcut with single distance parameter.
translate : Straight-line translation.
"""
nt = trajectory.displace(
self.traj,
forward=forward,
backward=backward,
start_idx=start_idx,
end_idx=end_idx,
)
return self._set(nt, needs_vel_update=True)
[docs]
@check_value_limits
@log_method
def move_local_with_speed(
self,
*,
start_idx: int,
speed: float,
axis: Axis = "x",
) -> "TrajectoryEditor":
"""Build a constant-speed motion transformation sequence.
Creates transformation matrices that move the object at
constant speed along a local axis from the specified frame.
Parameters
----------
start_idx : int
Frame index to start the motion.
speed : float
Speed in meters per second.
axis : {"x", "y", "z"}, optional
Local axis to move along. Default is ``"x"`` (forward).
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.move_local_with_speed(start_idx=10, speed=5.0) # 5 m/s forward
"""
ax = {"x": 0, "y": 1, "z": 2}[axis]
return trajectory.build_local_axis_speed_T_seq(
self.traj.timestamps, start_idx, speed=speed, axis=ax
)
[docs]
@check_value_limits
@log_method
def scale_speed(
self,
*,
multiplier: float,
anchor: Literal["start", "first"] = "start",
) -> "TrajectoryEditor":
"""Change apparent speed by retiming timestamps.
Adjusts the trajectory's timestamps to change the perceived
speed without modifying the spatial path. Frame count and
poses remain unchanged.
Parameters
----------
multiplier : float
Speed scaling factor:
- ``> 1``: Faster (shorter time between frames)
- ``< 1``: Slower (longer time between frames)
Must be positive.
anchor : {"start", "first"}, optional
Time reference point that remains fixed. Default is ``"start"``.
Returns
-------
TrajectoryEditor
Self for method chaining.
Raises
------
ValueError
If multiplier is not positive.
Examples
--------
>>> te.scale_speed(multiplier=2.0) # Double speed
>>> te.scale_speed(multiplier=0.5) # Half speed
"""
m = float(multiplier)
if m <= 0:
raise ValueError("multiplier must be > 0")
t = np.asarray(self.traj.timestamps, float)
if len(t) <= 1:
return self # nothing to do
if anchor not in ("start", "first"):
raise ValueError("anchor must be 'start'")
t0 = float(t[0])
new_t = t0 + (t - t0) / m
nt = self.traj.model_copy(deep=True)
nt.timestamps = new_t
return self._set(nt, needs_vel_update=True)
[docs]
@check_value_limits
@log_method
def lane_change(
self,
start_idx: int,
duration: int,
lateral: float,
kind: Literal["smooth", "abrupt", "evasive"] = "smooth",
) -> "TrajectoryEditor":
"""Build a lateral lane change maneuver.
Applies a smooth lateral displacement to simulate a lane
change, with optional tail offset persistence.
Parameters
----------
start_idx : int
Frame index to start the maneuver.
duration : int
Number of frames for the maneuver.
lateral : float
Lateral distance in meters. Positive is left.
kind : {"smooth", "abrupt", "evasive"}, optional
Type of lane change:
- ``"smooth"``: Gradual transition to new lane (step function).
- ``"abrupt"``: Quick transition.
- ``"evasive"``: Swerve and return to original lane (bump).
Default is ``"smooth"``.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.lane_change(start_idx=10, duration=30, lateral=3.5) # Left lane change
>>> te.lane_change(start_idx=20, duration=40, lateral=-3.5) # Right lane change
>>> te.lane_change(start_idx=30, duration=50, lateral=2.0, kind="evasive")
"""
Tw = np.asarray(self.traj.object2world, float)
T_seq, tail_y = build_lane_change_T_seq(
ego_object2world=Tw,
start_idx=int(start_idx),
duration=int(duration),
distance_m=float(lateral),
kind=str(kind),
)
i0 = int(start_idx)
i1 = i0 + int(duration)
self.apply_T_sequence(T_seq, start_idx=i0, end_idx=i1, referential="local")
# Persist tail offset (if any)
if i1 < len(Tw) and abs(tail_y) > 1e-12:
if tail_y >= 0:
self.transform(left=tail_y, start_idx=i1, end_idx=None)
else:
self.transform(right=-tail_y, start_idx=i1, end_idx=None)
return self
[docs]
def make_static_from_reference(
self,
reference: WorldTrajectory,
*,
at_idx: int = 0,
forward: float = 0.0,
backward: float = 0.0,
left: float = 0.0,
right: float = 0.0,
up: float = 0.0,
down: float = 0.0,
yaw_deg: float = 0.0,
timestamps: Optional[np.ndarray] = None,
) -> "TrajectoryEditor":
"""Create a static trajectory positioned relative to a reference.
Builds a trajectory where the object remains stationary at a
position determined by offsets from a reference trajectory
at a specific frame.
Parameters
----------
reference : WorldTrajectory
Reference trajectory for positioning.
at_idx : int, optional
Frame index in reference trajectory to use. Default is 0.
forward : float, optional
Forward offset from reference in meters. Default is 0.
backward : float, optional
Backward offset from reference in meters. Default is 0.
left : float, optional
Left offset from reference in meters. Default is 0.
right : float, optional
Right offset from reference in meters. Default is 0.
up : float, optional
Up offset from reference in meters. Default is 0.
down : float, optional
Down offset from reference in meters. Default is 0.
yaw_deg : float, optional
Yaw rotation relative to reference in degrees. Default is 0.
timestamps : np.ndarray | None, optional
Timestamps for the static trajectory. If None, uses
reference timestamps.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.make_static_from_reference(
... ego_traj,
... at_idx=0,
... forward=20,
... left=3.5
... )
"""
new_traj = trajectory.build_static_from_reference(
reference,
at_idx=at_idx,
forward=forward,
backward=backward,
left=left,
right=right,
up=up,
down=down,
yaw_deg=yaw_deg,
timestamps=timestamps,
)
return self.replace(new_traj)
# ---------- Smoothing / utils ----------
[docs]
@log_method
def smooth(
self,
*,
method: Literal["savgol"] = "savgol",
window: int = 11,
polyorder: int = 3,
) -> "TrajectoryEditor":
"""Smooth the trajectory positions using filtering.
Applies a smoothing filter to reduce noise in the
position components of the trajectory.
Parameters
----------
method : {"savgol"}, optional
Smoothing method. Currently only Savitzky-Golay is supported.
Default is ``"savgol"``.
window : int, optional
Window size for the filter. Must be odd. Default is 11.
polyorder : int, optional
Polynomial order for Savitzky-Golay. Default is 3.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.smooth() # Default parameters
>>> te.smooth(window=21, polyorder=5) # Custom smoothing
"""
nt = trajectory.smooth_positions_savgol(
self.traj, window=window, polyorder=polyorder
)
return self._set(nt, needs_vel_update=True)
[docs]
@log_method
def recompute_velocities(self) -> "TrajectoryEditor":
"""Recompute linear and angular velocities from poses.
Recalculates velocity arrays based on the current pose
sequence and timestamps. Useful after external modifications.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.recompute_velocities()
"""
lin, ang = estimate_velocities_world(self.traj)
self.traj.linear_velocity = lin
self.traj.angular_velocity = ang
return self
[docs]
@log_method
def replace(self, traj: WorldTrajectory) -> "TrajectoryEditor":
"""Replace the underlying trajectory.
Substitutes the trajectory being edited with a new one.
If ``recompute_on_mutation`` is True, velocities are
recomputed for the new trajectory.
Parameters
----------
traj : WorldTrajectory
The new trajectory to edit.
Returns
-------
TrajectoryEditor
Self for method chaining.
Examples
--------
>>> te.replace(new_trajectory)
"""
self.traj = traj
if self.recompute_on_mutation:
lin, ang = estimate_velocities_world(self.traj)
self.traj.linear_velocity = lin
self.traj.angular_velocity = ang
return self