Implementation:ARISE Initiative Robosuite TrajUtils
| Knowledge Sources | |
|---|---|
| Domains | Robotics, Trajectory Planning |
| Last Updated | 2026-02-15 07:00 GMT |
Overview
The traj_utils module provides the Interpolator interface and the LinearInterpolator class for smoothly interpolating between goal positions and orientations across controller timesteps, supporting both linear position interpolation and spherical linear interpolation (SLERP) for rotations.
Description
This module addresses the common problem in robotic control where the policy updates at a lower frequency than the controller. The interpolator bridges this frequency gap by generating smooth intermediate setpoints between successive policy commands.
Interpolator is an abstract base class defining a single method get_interpolated_goal that returns the next interpolated step.
LinearInterpolator is the concrete implementation that supports n-dimensional position interpolation and optional orientation interpolation. At construction, it takes the number of dimensions, controller frequency, policy frequency, and a ramp ratio that determines what fraction of the inter-policy interval is used for ramping (the total number of interpolation steps is ceil(ramp_ratio * controller_freq / policy_freq)). The ori_interpolate parameter specifies whether the interpolator handles orientations and in what representation ("euler" or "quat").
When a new goal is set via set_goal, the current goal becomes the start state and the interpolation step counter resets. On each get_interpolated_goal call, for position interpolation, the method computes a linear step dx = (goal - start) / (total_steps - step) and increments the step counter. For orientation interpolation, it uses quat_slerp from transform_utils to perform spherical linear interpolation (converting Euler angles to quaternions first if needed, then back after interpolation).
The step counter saturates at total_steps - 1, meaning the interpolator will hold the final goal value once fully ramped. The set_states method allows reconfiguring dimensions and interpolation mode after construction.
Usage
Use LinearInterpolator in controller implementations to smoothly interpolate between policy commands. Create separate instances for position and orientation interpolation. Call set_goal when a new policy action arrives, and call get_interpolated_goal at each controller timestep.
Code Reference
Source Location
- Repository: ARISE_Initiative_Robosuite
- File: robosuite/utils/traj_utils.py
Signature
class Interpolator(object, metaclass=abc.ABCMeta):
@abc.abstractmethod
def get_interpolated_goal(self) -> np.array: ...
class LinearInterpolator(Interpolator):
def __init__(
self,
ndim: int,
controller_freq: float,
policy_freq: float,
ramp_ratio: float = 0.2,
use_delta_goal: bool = False,
ori_interpolate: str = None,
): ...
def set_states(self, dim=None, ori=None): ...
def set_goal(self, goal: np.array): ...
def get_interpolated_goal(self) -> np.array: ...
Import
from robosuite.utils.traj_utils import LinearInterpolator
I/O Contract
Inputs
| Name | Type | Required | Description |
|---|---|---|---|
| ndim | int | Yes | Number of dimensions to interpolate (e.g., 3 for position, 4 for quaternion) |
| controller_freq | float | Yes | Frequency of the low-level controller in Hz |
| policy_freq | float | Yes | Frequency of the high-level policy in Hz |
| ramp_ratio | float | No | Fraction of inter-policy interval for ramping (default: 0.2) |
| use_delta_goal | bool | No | Whether to use delta goals (not yet implemented, default: False) |
| ori_interpolate | str or None | No | Orientation interpolation mode: None (position), "euler", or "quat" |
| goal (set_goal) | np.array | Yes | Target goal value to interpolate toward (absolute coordinates) |
Outputs
| Name | Type | Description |
|---|---|---|
| get_interpolated_goal | np.array | Next interpolated step toward the current goal |
Usage Examples
import numpy as np
from robosuite.utils.traj_utils import LinearInterpolator
# Create a position interpolator
# Controller runs at 500 Hz, policy at 20 Hz, ramp over 20% of the interval
pos_interpolator = LinearInterpolator(
ndim=3,
controller_freq=500,
policy_freq=20,
ramp_ratio=0.2,
ori_interpolate=None, # No orientation interpolation
)
# Set a new goal position (from policy command)
pos_interpolator.set_goal(np.array([0.5, 0.0, 0.8]))
# Get interpolated steps at each controller timestep
for i in range(25): # 500/20 = 25 controller steps per policy step
intermediate_pos = pos_interpolator.get_interpolated_goal()
print(f"Step {i}: {intermediate_pos}")
# Create an orientation interpolator using quaternions
ori_interpolator = LinearInterpolator(
ndim=4,
controller_freq=500,
policy_freq=20,
ramp_ratio=0.2,
ori_interpolate="quat",
)
# Set a quaternion goal (x, y, z, w)
ori_interpolator.set_goal(np.array([0.0, 0.0, 0.707, 0.707]))
# Get SLERP-interpolated orientations
for i in range(25):
intermediate_quat = ori_interpolator.get_interpolated_goal()
print(f"Step {i}: {intermediate_quat}")