[Diffusion] add GR00T-N1.7 pipeline with OpenPI serving (#3798)

Signed-off-by: Zhengyuan Su <su.zhengyuan@u.nus.edu>
Co-authored-by: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
Zhengyuan Su (苏政渊)andClaude Opus 4.8 authored and GitHub committed 2026-06-20 11:45:26 +08:00
1 parent e70822ed54
commit 2d0b1d8a29
34 files changed
+5495

No files matched your search

@@ -0,0 +1,2 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
+191
View File
@@ -0,0 +1,191 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
from enum import Enum
"""
Embodiment tags are used to identify the robot embodiment in the data.
Naming convention:
<dataset>_<robot_name>
If using multiple datasets, e.g. sim GR1 and real GR1, we can drop the dataset name and use only the robot name.
"""
class EmbodimentTag(Enum):
"""Embodiment tags supported by the GR00T N1.7 checkpoint.
Pretrain tags (baked into the base model nvidia/GR00T-N1.7-3B, inference-ready):
- OXE_DROID_RELATIVE_EEF_RELATIVE_JOINT -> "oxe_droid_relative_eef_relative_joint"
- XDOF -> "xdof_relative_eef_relative_joint"
- XDOF_SUBTASK -> "xdof_relative_eef_relative_joint_subtask"
- REAL_G1 -> "real_g1_relative_eef_relative_joints"
- REAL_R1_PRO_SHARPA -> "real_r1_pro_sharpa_relative_eef"
- REAL_R1_PRO_SHARPA_HUMAN -> "real_r1_pro_sharpa_relative_eef_human"
- REAL_R1_PRO_SHARPA_MAXINSIGHTS -> "real_r1_pro_sharpa_relative_eef_maxinsights"
- REAL_R1_PRO_SHARPA_MECKA -> "real_r1_pro_sharpa_relative_eef_mecka"
Pre-registered posttrain tags (require finetuned checkpoint):
- UNITREE_G1 -> "unitree_g1_full_body_with_waist_height_nav_cmd"
- UNITREE_G1_SONIC -> "unitree_g1_sonic"
- SIMPLER_ENV_GOOGLE -> "simpler_env_google"
- SIMPLER_ENV_WIDOWX -> "simpler_env_widowx"
- LIBERO_PANDA -> "libero_sim"
Finetuning tag (for custom robots):
- NEW_EMBODIMENT -> "new_embodiment"
Use ``EmbodimentTag.resolve(s)`` to look up a tag by name or value,
case-insensitively.
"""
OXE_DROID_RELATIVE_EEF_RELATIVE_JOINT = "oxe_droid_relative_eef_relative_joint"
"""
The Open-X-Embodiment DROID robot with relative EEF and relative joint position actions.
"""
XDOF = "xdof_relative_eef_relative_joint"
"""
The generic X-DOF robot with relative EEF and relative joint position actions.
"""
XDOF_SUBTASK = "xdof_relative_eef_relative_joint_subtask"
"""
The generic X-DOF robot (subtask variant).
"""
REAL_G1 = "real_g1_relative_eef_relative_joints"
"""
Real-world Unitree G1 with relative EEF and relative joint actions.
"""
REAL_R1_PRO_SHARPA = "real_r1_pro_sharpa_relative_eef"
"""
Real-world R1 Pro Sharpa with relative EEF actions.
"""
REAL_R1_PRO_SHARPA_HUMAN = "real_r1_pro_sharpa_relative_eef_human"
"""
Real-world R1 Pro Sharpa with relative EEF actions (human teleop data).
"""
REAL_R1_PRO_SHARPA_MAXINSIGHTS = "real_r1_pro_sharpa_relative_eef_maxinsights"
"""
Real-world R1 Pro Sharpa with relative EEF actions (MaxInsights data, single-cam).
"""
REAL_R1_PRO_SHARPA_MECKA = "real_r1_pro_sharpa_relative_eef_mecka"
"""
Real-world R1 Pro Sharpa with relative EEF actions (Mecka data, single-cam).
"""
UNITREE_G1 = "unitree_g1_full_body_with_waist_height_nav_cmd"
"""
The Unitree G1 robot (sim, full-body with waist height and nav commands).
"""
UNITREE_G1_SONIC = "unitree_g1_sonic"
"""
The Unitree G1 robot with SONIC whole-body controller. VLA action space is SONIC latents.
"""
SIMPLER_ENV_GOOGLE = "simpler_env_google"
"""
The SimplerEnv Google robot.
"""
SIMPLER_ENV_WIDOWX = "simpler_env_widowx"
"""
The SimplerEnv WidowX robot.
"""
LIBERO_PANDA = "libero_sim"
"""
The LIBERO Panda robot (used for LIBERO-Goal, LIBERO-Object, LIBERO-Spatial, LIBERO-10).
"""
# New embodiment during post-training
NEW_EMBODIMENT = "new_embodiment"
"""
Any new embodiment.
"""
@classmethod
def resolve(cls, tag: "str | EmbodimentTag") -> "EmbodimentTag":
"""Resolve a string to an EmbodimentTag, case-insensitively.
Matches by enum **name** first (e.g. ``"xdof"`` -> ``XDOF``), then by
enum **value** (e.g. ``"xdof_relative_eef_relative_joint"`` -> ``XDOF``).
Raises:
ValueError: If *tag* does not match any known embodiment.
"""
if isinstance(tag, cls):
return tag
key = tag.strip()
key_lower = key.lower()
# Match by enum name (case-insensitive)
for member in cls:
if member.name.lower() == key_lower:
return member
# Match by enum value (case-insensitive)
for member in cls:
if member.value.lower() == key_lower:
return member
def _fmt(tags):
return "\n".join(f" {m.name:40s} -> {m.value}" for m in tags)
msg = (
f"Unknown embodiment tag: {tag!r}\n\n"
f" Base model tags (work with nvidia/GR00T-N1.7-3B):\n"
f"{_fmt(PRETRAIN_TAGS)}\n\n"
f" Posttrain tags (require a finetuned checkpoint):\n"
f"{_fmt(POSTTRAIN_TAGS)}\n\n"
f" Finetuning-only tags (for custom robots):\n"
f"{_fmt(FINETUNE_ONLY_TAGS)}"
)
raise ValueError(msg)
@classmethod
def reverse_lookup(cls, value: str) -> "str":
"""Map a tag value string back to its enum name, or return the value as-is."""
for member in cls:
if member.value == value:
return member.name
return value
# Module-level tag category sets (cannot be Enum class attributes).
PRETRAIN_TAGS: frozenset[EmbodimentTag] = frozenset(
{
EmbodimentTag.OXE_DROID_RELATIVE_EEF_RELATIVE_JOINT,
EmbodimentTag.XDOF,
EmbodimentTag.XDOF_SUBTASK,
EmbodimentTag.REAL_G1,
EmbodimentTag.REAL_R1_PRO_SHARPA,
EmbodimentTag.REAL_R1_PRO_SHARPA_HUMAN,
EmbodimentTag.REAL_R1_PRO_SHARPA_MAXINSIGHTS,
EmbodimentTag.REAL_R1_PRO_SHARPA_MECKA,
}
)
"""Tags baked into the base model (nvidia/GR00T-N1.7-3B) — usable without finetuning."""
POSTTRAIN_TAGS: frozenset[EmbodimentTag] = frozenset(
{
EmbodimentTag.UNITREE_G1,
EmbodimentTag.UNITREE_G1_SONIC,
EmbodimentTag.SIMPLER_ENV_GOOGLE,
EmbodimentTag.SIMPLER_ENV_WIDOWX,
EmbodimentTag.LIBERO_PANDA,
}
)
"""Tags that require a finetuned checkpoint."""
FINETUNE_ONLY_TAGS: frozenset[EmbodimentTag] = frozenset(
{
EmbodimentTag.NEW_EMBODIMENT,
}
)
"""Tags for custom robots (finetuning only, not in any shipped checkpoint)."""
@@ -0,0 +1,2 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
@@ -0,0 +1,654 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
from collections.abc import Sequence
from typing import Generic, TypeVar
import numpy as np
from numpy.typing import NDArray
from scipy import interpolate
from scipy.spatial.transform import Rotation, Slerp
from vllm_omni.diffusion.models.gr00t.dataio.state_action.pose import EndEffectorPose, JointPose, Pose
from vllm_omni.diffusion.models.gr00t.dataio.types import ActionFormat
PoseType = TypeVar("PoseType", bound=Pose)
class ActionChunk(Generic[PoseType]):
"""
Abstract base class for robot action chunking.
This class provides common functionality for different action chunking types
including relative and delta action chunking computation with optional reference frames,
interpolation, and format conversion.
"""
def __init__(
self,
poses: Sequence[PoseType],
times: Sequence[float] | NDArray[np.float64] | None = None,
):
"""
Initialize action chunking from a list of poses.
Args:
poses: Sequence of Pose objects
times: Optional sequence of timestamps for each pose. If None, assumes
uniform spacing starting from 0 with step 1.0
Raises:
ValueError: If action chunking is empty or times length doesn't match poses
"""
if not poses:
raise ValueError("ActionChunk must contain at least one pose")
self._poses: list[PoseType] = list(poses)
# Set up times
if times is None:
self._times = np.arange(len(poses), dtype=np.float64)
else:
if len(times) != len(poses):
raise ValueError("Number of times must match number of poses")
self._times = np.array(times, dtype=np.float64)
@property
def poses(self) -> list[PoseType]:
"""Get the list of poses"""
return self._poses.copy()
@property
def times(self) -> NDArray[np.float64]:
"""Get the timestamps"""
return self._times.copy()
@property
def num_poses(self) -> int:
"""Get the number of poses in the action chunking"""
return len(self._poses)
def relative_chunking(self, reference_frame: PoseType | None = None) -> "ActionChunk[PoseType]":
"""
Compute the relative action chunking with respect to a reference frame.
If reference_frame is None, uses the first pose in the action chunking as reference.
All poses are transformed to be relative to the reference frame.
Args:
reference_frame: Optional reference pose. If None, uses first pose.
Returns:
A new ActionChunk of the same type where all poses are relative to the reference frame.
"""
if not self._poses:
return self.__class__([], times=[])
# Use the first pose as the reference if one is not provided.
ref_pose = reference_frame if reference_frame is not None else self._poses[0]
# Use the polymorphic subtraction defined in the Pose subclasses.
# The subtraction returns the same type as the operands
relative_poses: list[PoseType] = [pose - ref_pose for pose in self._poses] # type: ignore[misc]
# Return a new instance of the same action chunking class
# (e.g., JointActionChunk or EndEffectorActionChunk)
return self.__class__(relative_poses, times=self.times)
def delta_chunking(self, reference_frame: PoseType | None = None) -> "ActionChunk[PoseType]":
"""
Compute the delta action chunking where each pose represents the relative
transformation from the previous frame.
If reference_frame is provided, it is treated as the first frame, and the
first delta will be from reference_frame to the first pose in the action chunking.
Otherwise, the first pose in the delta action chunking will be the identity/zero transformation.
Args:
reference_frame: Optional reference pose to use as the first frame.
Returns:
A new ActionChunk of the same type where each pose is relative to the previous pose.
"""
if not self._poses:
return self.__class__([], times=[])
delta_poses: list[PoseType] = []
# Determine the initial reference for the very first pose.
# If a reference_frame is given, the first delta is pose[0] - reference_frame.
# If not, the first delta is pose[0] - pose[0], resulting in an identity/zero pose.
prev_pose = reference_frame if reference_frame is not None else self._poses[0]
for current_pose in self._poses:
delta: PoseType = current_pose - prev_pose # type: ignore[assignment]
delta_poses.append(delta)
prev_pose = current_pose # Update the reference for the next step
return self.__class__(delta_poses, times=self.times.tolist())
def to_absolute_chunking(self, reference_frame: PoseType) -> "ActionChunk[PoseType]":
"""
Convert a relative action chunking to an absolute action chunking by applying
the relative poses on top of a reference frame.
This is the inverse operation of relative_chunking(). Each relative pose
is composed with the reference frame to produce absolute poses.
Args:
reference_frame: The reference pose to apply the relative action chunking on top of.
Returns:
A new ActionChunk of the same type with absolute poses.
Raises:
NotImplementedError: Must be implemented by subclasses
"""
raise NotImplementedError("Subclasses must implement to_absolute_chunking")
def interpolate(
self,
num_points: int | None = None,
times: NDArray[np.float64] | None = None,
) -> "ActionChunk":
"""
Interpolate the action chunking to generate intermediate poses.
Must be implemented by subclasses.
Args:
num_points: Number of evenly-spaced points to generate
times: Specific timestamps at which to interpolate
Returns:
A new ActionChunk with interpolated poses
"""
raise NotImplementedError("Subclasses must implement interpolate")
def to(self, action_format: ActionFormat) -> NDArray[np.float64]:
"""
Convert action chunking to the specified action format.
Must be implemented by subclasses.
Args:
action_format: The desired output format
Returns:
Array in the requested format
Raises:
NotImplementedError: If not implemented by subclass
"""
raise NotImplementedError("Subclasses must implement to method")
def __len__(self) -> int:
"""Return the number of poses in the action chunking"""
return len(self._poses)
def __getitem__(self, index: int) -> PoseType:
"""Get a pose by index"""
return self._poses[index]
def __repr__(self) -> str:
return (
f"{self.__class__.__name__}(num_poses={len(self._poses)}, "
f"time_range=[{self._times[0]:.2f}, {self._times[-1]:.2f}])"
)
class JointActionChunk(ActionChunk[JointPose]):
"""
Represents action chunking in joint space as a sequence of joint configurations.
Examples:
# Create a joint action chunking
joint_poses = [
JointPose([0.0, 0.0, 0.0]),
JointPose([0.5, 0.5, 0.5]),
JointPose([1.0, 1.0, 1.0]),
]
action_chunking = JointActionChunk(joint_poses)
# Get relative trajectory (all poses relative to first pose)
relative_traj = action_chunking.relative_chunking()
# Get relative trajectory with custom reference
reference = JointPose([0.1, 0.1, 0.1])
relative_traj = action_chunking.relative_chunking(reference_frame=reference)
# Get delta trajectory (incremental changes)
delta_traj = action_chunking.delta_chunking()
# Convert relative trajectory back to absolute
reference = JointPose([0.1, 0.1, 0.1])
absolute_traj = relative_traj.to_absolute_chunking(reference_frame=reference)
# Interpolate trajectory
interpolated = action_chunking.interpolate(num_points=10)
# Convert to desired format
from vllm_omni.diffusion.models.gr00t.dataio.types import ActionFormat
array_data = action_chunking.to(ActionFormat.DEFAULT) # Returns joint array
"""
def __init__(
self,
poses: Sequence[JointPose],
times: Sequence[float] | NDArray[np.float64] | None = None,
):
"""
Initialize a joint trajectory from a list of joint poses.
Args:
poses: Sequence of JointPose objects
times: Optional sequence of timestamps for each pose
Raises:
TypeError: If poses are not all JointPose objects
"""
# Validate all poses are JointPose
if not all(isinstance(p, JointPose) for p in poses):
raise TypeError("All poses must be JointPose objects for JointActionChunk")
super().__init__(poses, times)
def interpolate(
self,
num_points: int | None = None,
times: NDArray[np.float64] | None = None,
) -> "JointActionChunk":
"""
Interpolate the joint action chunking to generate intermediate configurations.
Uses linear interpolation for joint values.
Args:
num_points: Number of evenly-spaced points to generate (including endpoints).
Only used if times is None.
times: Specific timestamps at which to interpolate. If provided,
num_points is ignored.
Returns:
A new JointActionChunk with interpolated poses
Raises:
ValueError: If neither num_points nor times is provided, or if
interpolation times are outside the trajectory range
"""
if num_points is None and times is None:
raise ValueError("Must provide either num_points or times")
if len(self._poses) < 2:
raise ValueError("Need at least 2 poses for interpolation")
# Prepare data: extract joint values
timestamps = self._times.copy()
joint_values = np.array([pose.joints for pose in self._poses]) # (N, num_joints)
# Find and remove non-monotonic timestamps
drop_indices = [idx for idx in range(1, len(timestamps)) if timestamps[idx] <= timestamps[idx - 1]]
if drop_indices:
for idx in drop_indices:
print(
f"Dropping timestamp pair - Previous: {timestamps[idx - 1]}, "
f"Current: {timestamps[idx]} at index {idx}"
)
timestamps = np.delete(timestamps, drop_indices)
joint_values = np.delete(joint_values, drop_indices, axis=0)
# Check if we still have enough poses after cleanup
if len(timestamps) < 2:
raise ValueError("Need at least 2 poses with monotonic timestamps for interpolation")
# Create interpolator
joint_interp = interpolate.interp1d(timestamps, joint_values, kind="linear", axis=0)
# Generate interpolation times if not provided
if times is None:
assert num_points is not None # Type narrowing for type checker
interp_times = np.linspace(timestamps[0], timestamps[-1], num_points)
else:
interp_times = np.array(times, dtype=np.float64)
# Check that interpolation times are within bounds
if np.any(interp_times < timestamps[0]) or np.any(interp_times > timestamps[-1]):
raise ValueError(f"Interpolation times must be within [{timestamps[0]}, {timestamps[-1]}]")
# Interpolate joint values
interp_joint_values = joint_interp(interp_times)
# Create interpolated poses
joint_names = self._poses[0].joint_names
interpolated_poses = [
JointPose(joints=interp_joint_values[i], joint_names=joint_names) for i in range(len(interp_times))
]
return JointActionChunk(interpolated_poses, times=interp_times)
def to_array(self) -> NDArray[np.float64]:
"""
Convert trajectory to array of joint values.
Returns:
Array with shape (N, num_joints) where N is the number of poses
"""
return np.array([pose.joints for pose in self._poses])
def to_absolute_chunking(self, reference_frame: JointPose) -> "JointActionChunk":
"""
Convert a relative joint action chunking to an absolute action chunking by adding
the relative joint positions to the reference frame.
This is the inverse operation of relative_chunking(). Each relative joint
configuration is added to the reference frame to produce absolute joint positions.
Args:
reference_frame: The reference joint pose to apply the relative trajectory on top of.
Returns:
A new JointActionChunk with absolute joint positions.
Raises:
ValueError: If joint dimensions don't match
"""
if not self._poses:
return JointActionChunk([], times=[])
if len(self._poses[0].joints) != len(reference_frame.joints):
raise ValueError(
f"Cannot apply relative trajectory: "
f"joint dimensions don't match ({len(self._poses[0].joints)} vs {len(reference_frame.joints)})"
)
# Add each relative pose to the reference frame
absolute_poses: list[JointPose] = []
for relative_pose in self._poses:
absolute_joints = reference_frame.joints + relative_pose.joints
absolute_pose = JointPose(joints=absolute_joints, joint_names=reference_frame.joint_names)
absolute_poses.append(absolute_pose)
return JointActionChunk(absolute_poses, times=self.times)
def to(self, action_format: ActionFormat) -> NDArray[np.float64]:
"""
Convert trajectory to the desired format.
Args:
action_format: The desired output format
Returns:
Array in the requested format
Raises:
ValueError: If the action format is not supported for joint trajectories
"""
if action_format == ActionFormat.DEFAULT:
return self.to_array()
else:
raise ValueError(
f"ActionFormat {action_format} is not supported for JointActionChunk. "
f"Only {ActionFormat.DEFAULT} is supported."
)
class EndEffectorActionChunk(ActionChunk[EndEffectorPose]):
"""
Represents action chunking in Cartesian space as a sequence of end-effector poses.
Examples:
# Create an end-effector action chunking
ee_poses = [
EndEffectorPose(translation=[0, 0, 0], rotation=[1, 0, 0, 0],
rotation_type="quat", rotation_order="wxyz"),
EndEffectorPose(translation=[1, 0, 0], rotation=[0.707, 0, 0, 0.707],
rotation_type="quat", rotation_order="wxyz"),
EndEffectorPose(translation=[2, 0, 0], rotation=[0, 0, 0, 1],
rotation_type="quat", rotation_order="wxyz"),
]
action_chunking = EndEffectorActionChunk(ee_poses)
# Get relative trajectory (all poses relative to first pose)
relative_traj = action_chunking.relative_chunking()
# Get relative trajectory with custom reference frame
reference = EndEffectorPose(translation=[0.5, 0, 0], rotation=[1,0,0,0],
rotation_type="quat", rotation_order="wxyz")
relative_traj = action_chunking.relative_chunking(reference_frame=reference)
# Get delta trajectory
delta_traj = action_chunking.delta_chunking()
# Convert relative trajectory back to absolute
reference = EndEffectorPose(translation=[0.5, 0, 0], rotation=[1,0,0,0],
rotation_type="quat", rotation_order="wxyz")
absolute_traj = relative_traj.to_absolute_chunking(reference_frame=reference)
# Interpolate trajectory
interpolated = action_chunking.interpolate(num_points=10)
# Convert to desired format
from vllm_omni.diffusion.models.gr00t.dataio.types import ActionFormat
homo_matrices = action_chunking.to(ActionFormat.DEFAULT) # (N, 4, 4) homogeneous matrices
xyz_rot6d = action_chunking.to(ActionFormat.XYZ_ROT6D) # (N, 9) xyz + rot6d
xyz_rotvec = action_chunking.to(ActionFormat.XYZ_ROTVEC) # (N, 6) xyz + rotvec
"""
def __init__(
self,
poses: Sequence[EndEffectorPose],
times: Sequence[float] | NDArray[np.float64] | None = None,
):
"""
Initialize an end-effector trajectory from a list of end-effector poses.
Args:
poses: Sequence of EndEffectorPose objects
times: Optional sequence of timestamps for each pose
Raises:
TypeError: If poses are not all EndEffectorPose objects
"""
# Validate all poses are EndEffectorPose
if not all(isinstance(p, EndEffectorPose) for p in poses):
raise TypeError("All poses must be EndEffectorPose objects for EndEffectorActionChunk")
super().__init__(poses, times)
@classmethod
def from_array(cls, data: np.ndarray, action_format: ActionFormat) -> "EndEffectorActionChunk":
"""
Create an EndEffectorActionChunk from a 2-D array using the specified action format.
This is the inverse of ``.to(action_format)``.
Args:
data: Array of shape (N, D) where D depends on the action_format.
action_format: The format that describes the layout of each row.
Returns:
EndEffectorActionChunk with N poses.
"""
poses = [EndEffectorPose.from_action_format(row, action_format) for row in data]
return cls(poses)
def interpolate(
self,
num_points: int | None = None,
times: NDArray[np.float64] | None = None,
) -> "EndEffectorActionChunk":
"""
Interpolate the action chunking to generate intermediate poses.
Uses linear interpolation for translation and SLERP (Spherical Linear
Interpolation) for rotation.
Args:
num_points: Number of evenly-spaced points to generate (including endpoints).
Only used if times is None.
times: Specific timestamps at which to interpolate. If provided,
num_points is ignored.
Returns:
A new EndEffectorActionChunk with interpolated poses
Raises:
ValueError: If neither num_points nor times is provided, or if
interpolation times are outside the trajectory range
"""
if num_points is None and times is None:
raise ValueError("Must provide either num_points or times")
if len(self._poses) < 2:
raise ValueError("Need at least 2 poses for interpolation")
# Prepare data: extract positions and rotations
timestamps = self._times.copy()
homogeneous_matrices = np.array([pose.homogeneous for pose in self._poses])
positions = homogeneous_matrices[:, :3, 3]
rotations = Rotation.from_matrix(homogeneous_matrices[:, :3, :3])
# Find indices where timestamps are not monotonically increasing
drop_indices = [idx for idx in range(1, len(timestamps)) if timestamps[idx] <= timestamps[idx - 1]]
# Remove the problematic timestamps and corresponding data
if drop_indices:
for idx in drop_indices:
print(
f"Dropping timestamp pair - Previous: {timestamps[idx - 1]}, "
f"Current: {timestamps[idx]} at index {idx}"
)
timestamps = np.delete(timestamps, drop_indices)
positions = np.delete(positions, drop_indices, axis=0)
rotations = Rotation.from_matrix(np.delete(homogeneous_matrices[:, :3, :3], drop_indices, axis=0))
# Check if we still have enough poses after cleanup
if len(timestamps) < 2:
raise ValueError("Need at least 2 poses with monotonic timestamps for interpolation")
# Create interpolators
pos_interp = interpolate.interp1d(timestamps, positions, kind="linear", axis=0)
rot_interp = Slerp(timestamps, rotations)
# Generate interpolation times if not provided
if times is None:
assert num_points is not None # Type narrowing for type checker
interp_times = np.linspace(timestamps[0], timestamps[-1], num_points)
else:
interp_times = np.array(times, dtype=np.float64)
# Check that interpolation times are within bounds
if np.any(interp_times < timestamps[0]) or np.any(interp_times > timestamps[-1]):
raise ValueError(f"Interpolation times must be within [{timestamps[0]}, {timestamps[-1]}]")
# Interpolate positions and rotations
interp_positions = pos_interp(interp_times)
interp_rotations = rot_interp(interp_times)
# Create interpolated poses
interpolated_poses = []
for i in range(len(interp_times)):
pose = EndEffectorPose(
translation=interp_positions[i],
rotation=interp_rotations[i].as_matrix(),
rotation_type="matrix",
)
interpolated_poses.append(pose)
return EndEffectorActionChunk(interpolated_poses, times=interp_times)
def to_homogeneous_matrices(self) -> NDArray[np.float64]:
"""
Convert trajectory to array of homogeneous transformation matrices.
Returns:
Array of homogeneous matrices with shape (N, 4, 4) where N is the number of poses
"""
return np.array([pose.homogeneous for pose in self._poses])
def to_translation_rot6d(self) -> NDArray[np.float64]:
"""
Convert trajectory to array of translations and 6D rotations.
Returns:
Array with shape (N, 9) - 3 for xyz + 6 for rot6d
"""
translations = np.array([pose.translation for pose in self._poses]) # (N, 3)
rotations = np.array([pose.rot6d for pose in self._poses]) # (N, 6)
# Concatenate translation and rotation
xyz_rot6d = np.concatenate([translations, rotations], axis=1) # (N, 9)
return xyz_rot6d
def to_translation_rotvec(self) -> NDArray[np.float64]:
"""
Convert trajectory to array of translations and rotation vectors.
Returns:
Array with shape (N, 6) - 3 for xyz + 3 for rotvec
"""
translations = np.array([pose.translation for pose in self._poses]) # (N, 3)
rotations = np.array([pose.rotvec for pose in self._poses]) # (N, 3)
# Concatenate translation and rotation
xyz_rotvec = np.concatenate([translations, rotations], axis=1) # (N, 6)
return xyz_rotvec
def to_absolute_chunking(self, reference_frame: EndEffectorPose) -> "EndEffectorActionChunk":
"""
Convert a relative end-effector action chunking to an absolute action chunking by
composing each relative transformation with the reference frame.
This is the inverse operation of relative_chunking(). Each relative pose
represents a transformation that is applied on top of the reference frame
to produce absolute poses.
Args:
reference_frame: The reference end-effector pose to apply the relative trajectory on top of.
Returns:
A new EndEffectorActionChunk with absolute poses.
"""
if not self._poses:
return EndEffectorActionChunk([], times=[])
# Get reference frame as homogeneous matrix
T_ref = reference_frame.homogeneous
# Compose each relative transformation with the reference frame
absolute_poses: list[EndEffectorPose] = []
for relative_pose in self._poses:
# Get relative transformation as homogeneous matrix
T_relative = relative_pose.homogeneous
# Compose transformations: T_absolute = T_ref @ T_relative
T_absolute = T_ref @ T_relative
# Create absolute pose from composed transformation
absolute_pose = EndEffectorPose(homogeneous=T_absolute)
absolute_poses.append(absolute_pose)
return EndEffectorActionChunk(absolute_poses, times=self.times)
def to(self, action_format: ActionFormat) -> NDArray[np.float64]:
"""
Convert trajectory to the desired format.
Args:
action_format: The desired output format
Returns:
Array in the requested format
Raises:
ValueError: If the action format is not supported
"""
if action_format == ActionFormat.DEFAULT:
return self.to_homogeneous_matrices()
elif action_format == ActionFormat.XYZ_ROT6D:
return self.to_translation_rot6d()
elif action_format == ActionFormat.XYZ_ROTVEC:
return self.to_translation_rotvec()
else:
raise ValueError(f"Unsupported action format: {action_format}")
@@ -0,0 +1,709 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
from enum import Enum
from typing import TypeVar
import numpy as np
from numpy.typing import NDArray
from scipy.spatial.transform import Rotation
from vllm_omni.diffusion.models.gr00t.dataio.types import ActionFormat
# TypeVar for self-type preservation in Pose operations
PoseT = TypeVar("PoseT", bound="Pose")
def invert_transformation(transform: NDArray[np.float64]) -> NDArray[np.float64]:
"""
Invert a homogeneous transformation matrix.
Args:
transform: A 4x4 homogeneous transformation matrix
Returns:
The inverse of the transformation matrix (4x4)
"""
R = transform[:3, :3] # Extract the rotation matrix
t = transform[:3, 3] # Extract the translation vector
# Inverse of the rotation matrix is its transpose (since it's orthogonal)
R_inv = R.T
# Inverse of the translation is -R_inv * t
t_inv = -R_inv @ t
# Construct the inverse transformation matrix
T_inv = np.eye(4)
T_inv[:3, :3] = R_inv
T_inv[:3, 3] = t_inv
return T_inv
def relative_transformation(
base_transform: NDArray[np.float64], target_transform: NDArray[np.float64]
) -> NDArray[np.float64]:
"""
Compute the relative transformation between two poses.
Args:
base_transform: Initial 4x4 homogeneous transformation matrix
target_transform: Current 4x4 homogeneous transformation matrix
Returns:
The relative transformation matrix (4x4) from base_transform to target_transform
"""
# Relative transformation is base_transform^{-1} * target_transform
T_relative = invert_transformation(base_transform) @ target_transform
return T_relative
class RotationType(Enum):
"""Supported rotation representation types"""
QUAT = "quat"
EULER = "euler"
ROTVEC = "rotvec"
MATRIX = "matrix"
ROT6D = "rot6d"
class EulerOrder(Enum):
"""Common Euler angle conventions"""
XYZ = "xyz"
ZYX = "zyx"
XZY = "xzy"
YXZ = "yxz"
YZX = "yzx"
ZXY = "zxy"
class QuatOrder(Enum):
"""Quaternion ordering conventions"""
WXYZ = "wxyz" # scalar-first (w, x, y, z)
XYZW = "xyzw" # scalar-last (x, y, z, w)
class Pose:
"""
Abstract base class for robot poses.
This class provides common functionality for different pose representations
including relative pose computation via the subtraction operator.
"""
pose_type: str
def __sub__(self: PoseT, other: PoseT) -> PoseT:
"""
Compute relative transformation between two poses.
For EndEffectorPose: Computes the relative transformation from other to self.
Result represents the transformation needed to go from other's frame to self's frame.
For JointPose: Computes the joint-space difference (self - other).
Args:
other: The reference pose to compute relative transformation from
Returns:
Relative pose (same type as self)
Raises:
TypeError: If poses are not of the same type
Examples:
# End-effector poses
pose1 = EndEffectorPose(translation=[1, 0, 0], rotation=[1,0,0,0],
rotation_type="quat", rotation_order="wxyz")
pose2 = EndEffectorPose(translation=[2, 0, 0], rotation=[1,0,0,0],
rotation_type="quat", rotation_order="wxyz")
relative = pose2 - pose1 # Transformation from pose1 to pose2
# Joint poses
joint1 = JointPose([0.0, 0.5, 1.0])
joint2 = JointPose([0.1, 0.6, 1.2])
joint_diff = joint2 - joint1 # Joint differences: [0.1, 0.1, 0.2]
"""
if type(self) is not type(other):
raise TypeError(
f"Cannot compute relative transformation between different pose types: "
f"{type(self).__name__} and {type(other).__name__}"
)
return self._compute_relative(other)
def _compute_relative(self: PoseT, other: PoseT) -> PoseT:
"""
Internal method to compute relative transformation.
Must be implemented by subclasses.
Args:
other: The reference pose
Returns:
Relative pose
"""
raise NotImplementedError("Subclasses must implement _compute_relative")
def copy(self: PoseT) -> PoseT:
"""
Create a deep copy of this pose.
Must be implemented by subclasses.
Returns:
New Pose instance with copied data
"""
raise NotImplementedError("Subclasses must implement copy")
class JointPose(Pose):
"""
Represents a robot configuration in joint space.
This class stores joint angles/positions for a robot manipulator.
Unlike end-effector poses, joint poses represent the configuration
of all joints in the kinematic chain.
Examples:
# Create a 6-DOF joint configuration
joint_pose = JointPose(
joints=[0.0, -np.pi/4, np.pi/2, 0.0, np.pi/4, 0.0],
joint_names=["shoulder_pan", "shoulder_lift", "elbow",
"wrist_1", "wrist_2", "wrist_3"]
)
# Create with default joint names
joint_pose = JointPose(joints=[0.0, 0.5, 1.0])
# Get as dictionary
joint_dict = joint_pose.to_dict() # {"joint_0": 0.0, ...}
# Access individual joints
first_joint = joint_pose.joints[0]
num_joints = joint_pose.num_joints
# Compute relative joint displacement
joint1 = JointPose([0.0, 0.5, 1.0])
joint2 = JointPose([0.1, 0.6, 1.2])
relative = joint2 - joint1 # [0.1, 0.1, 0.2]
"""
pose_type = "joint"
def __init__(
self,
joints: list | np.ndarray,
joint_names: list | None = None,
):
"""
Initialize a joint pose.
Args:
joints: Joint angles/positions as array-like of shape (n,)
joint_names: Optional list of names for each joint. If None,
defaults to ["joint_0", "joint_1", ...]
"""
super().__init__()
self.joints = np.array(joints, dtype=np.float64)
# Set defaults and validate joint_names
if joint_names is None:
self.joint_names = [f"joint_{i}" for i in range(len(self.joints))]
else:
if len(joint_names) != len(self.joints):
raise ValueError(
f"Number of joint names ({len(joint_names)}) must match number of joints ({len(self.joints)})"
)
self.joint_names = joint_names
@property
def num_joints(self) -> int:
"""
Get the number of joints.
Returns:
Number of joints in the configuration
"""
return len(self.joints)
def to_dict(self) -> dict:
"""
Convert joint configuration to dictionary.
Returns:
Dictionary mapping joint names to joint values
"""
return dict(zip(self.joint_names, self.joints))
def _compute_relative(self, other): # type: ignore[override]
"""
Compute relative joint displacement.
Args:
other: Reference joint pose
Returns:
JointPose representing the joint-space difference (self - other)
Raises:
ValueError: If joint dimensions don't match
"""
if len(self.joints) != len(other.joints):
raise ValueError(
f"Cannot compute relative joint pose: "
f"joint dimensions don't match ({len(self.joints)} vs {len(other.joints)})"
)
relative_joints = self.joints - other.joints
return JointPose(joints=relative_joints, joint_names=self.joint_names)
def copy(self) -> "JointPose":
"""
Create a deep copy of this joint pose.
Returns:
New JointPose instance with copied data
"""
return JointPose(
joints=self.joints.copy(),
joint_names=self.joint_names.copy(),
)
def __repr__(self) -> str:
if len(self.joints) <= 6:
joints_str = np.array2string(self.joints, precision=4, suppress_small=True)
else:
joints_str = f"[{self.joints[0]:.4f}, ..., {self.joints[-1]:.4f}] ({len(self.joints)} joints)"
return f"JointPose(joints={joints_str})"
def __eq__(self, other) -> bool:
if not isinstance(other, JointPose):
return False
return np.allclose(self.joints, other.joints) and self.joint_names == other.joint_names
def __getitem__(self, index) -> float | NDArray[np.float64]:
"""Allow indexing: joint_pose[0] returns first joint value"""
return self.joints[index]
def __len__(self) -> int:
"""Allow len(): len(joint_pose) returns number of joints"""
return len(self.joints)
class EndEffectorPose(Pose):
"""
Represents a single end-effector pose with translation and rotation components.
This class handles Cartesian space representations of robot end-effector poses,
supporting multiple rotation representations (quaternions, Euler angles, rotation
vectors, rotation matrices, etc.).
Examples:
# Create with quaternion (wxyz order)
pose = EndEffectorPose(
translation=[1.0, 2.0, 3.0],
rotation=[1.0, 0.0, 0.0, 0.0],
rotation_type="quat",
rotation_order="wxyz"
)
# Create with Euler angles (degrees by default)
pose = EndEffectorPose(
translation=[1, 2, 3],
rotation=[0, 0, 90],
rotation_type="euler",
rotation_order="xyz"
)
# Create with Euler angles in radians
pose = EndEffectorPose(
translation=[1, 2, 3],
rotation=[0, 0, np.pi/2],
rotation_type="euler",
rotation_order="xyz",
degrees=False
)
# Create from homogeneous matrix
H = np.eye(4)
H[:3, 3] = [1, 2, 3]
pose = EndEffectorPose(homogeneous=H)
# Convert between representations
quat_wxyz = pose.to_rotation("quat", "wxyz")
euler_zyx = pose.to_rotation("euler", "zyx")
rot6d = pose.to_rotation("rot6d")
# Compute relative transformation
pose1 = EndEffectorPose(translation=[1, 0, 0], rotation=[1,0,0,0],
rotation_type="quat", rotation_order="wxyz")
pose2 = EndEffectorPose(translation=[2, 0, 0], rotation=[1,0,0,0],
rotation_type="quat", rotation_order="wxyz")
relative = pose2 - pose1 # Transformation from pose1's frame to pose2's frame
"""
pose_type = "end_effector"
def __init__(
self,
translation: list | np.ndarray | None = None,
rotation: list | np.ndarray | None = None,
rotation_type: str | None = None,
rotation_order: str | None = None,
homogeneous: np.ndarray | None = None,
degrees: bool = True,
):
"""
Initialize an end-effector pose.
Args:
translation: Translation vector [x, y, z]
rotation: Rotation in specified format
rotation_type: Type of rotation ("quat", "euler", "rotvec", "matrix", "rot6d")
rotation_order: Order/convention for the rotation type
homogeneous: Homogeneous transformation matrix (4, 4)
If provided, overrides translation and rotation
degrees: For Euler angles, whether the input is in degrees (default True)
"""
super().__init__()
# Cache for homogeneous matrix
self._homogeneous_cache: NDArray[np.float64] | None = None
self._cache_valid = False
# Handle homogeneous matrix input
if homogeneous is not None:
self._from_homogeneous(homogeneous)
return
# Store translation
self._translation = np.array(translation) if translation is not None else np.zeros(3)
# Store rotation as scipy Rotation object internally
if rotation is not None:
if rotation_type is None:
raise ValueError("rotation_type must be specified when rotation is provided")
self._set_rotation(rotation, rotation_type, rotation_order, degrees)
else:
self._rotation = Rotation.identity()
def _from_homogeneous(self, homogeneous: np.ndarray):
"""Initialize from homogeneous transformation matrix"""
homogeneous = np.array(homogeneous)
# Extract translation (last column, first 3 rows)
self._translation = homogeneous[:3, 3]
# Extract rotation matrix (top-left 3x3)
rotation_matrix = homogeneous[:3, :3]
# Create Rotation object from matrix
self._rotation = Rotation.from_matrix(rotation_matrix)
@staticmethod
def _rot6d_to_matrix(rot6d: np.ndarray) -> np.ndarray:
"""
Convert 6D rotation representation to rotation matrix.
Args:
rot6d: 6D rotation as (6,) array - first two rows of rotation matrix flattened
Returns:
Rotation matrix (3, 3)
"""
rot6d = rot6d.reshape(2, 3)
# First two rows
row1 = rot6d[0]
row2 = rot6d[1]
# Normalize first row
row1 = row1 / np.linalg.norm(row1)
# Gram-Schmidt orthogonalization for second row
row2 = row2 - np.dot(row1, row2) * row1
row2 = row2 / np.linalg.norm(row2)
# Third row is cross product
row3 = np.cross(row1, row2)
# Construct rotation matrix
rotation_matrix = np.vstack([row1, row2, row3])
return rotation_matrix
@staticmethod
def _matrix_to_rot6d(rotation_matrix: np.ndarray) -> np.ndarray:
"""
Convert rotation matrix to 6D rotation representation.
Args:
rotation_matrix: Rotation matrix (3, 3)
Returns:
6D rotation - (6,) array (first two rows flattened)
"""
return rotation_matrix[:2, :].flatten()
def _set_rotation(
self,
rotation: list | np.ndarray,
rotation_type: str,
rotation_order: str | None = None,
degrees: bool = True,
):
"""Internal method to set rotation from various representations"""
rotation = np.array(rotation)
rot_type = RotationType(rotation_type.lower())
if rot_type == RotationType.QUAT:
quat_order = QuatOrder(rotation_order.lower()) if rotation_order else QuatOrder.WXYZ
if quat_order == QuatOrder.WXYZ:
# scipy uses xyzw order, so convert
quat_xyzw = np.array([rotation[1], rotation[2], rotation[3], rotation[0]])
else:
quat_xyzw = rotation
self._rotation = Rotation.from_quat(quat_xyzw)
elif rot_type == RotationType.EULER:
euler_order = EulerOrder(rotation_order.lower()) if rotation_order else EulerOrder.XYZ
self._rotation = Rotation.from_euler(euler_order.value, rotation, degrees=degrees)
elif rot_type == RotationType.ROTVEC:
self._rotation = Rotation.from_rotvec(rotation)
elif rot_type == RotationType.MATRIX:
self._rotation = Rotation.from_matrix(rotation)
elif rot_type == RotationType.ROT6D:
rotation_matrix = self._rot6d_to_matrix(rotation)
self._rotation = Rotation.from_matrix(rotation_matrix)
else:
raise ValueError(f"Unknown rotation type: {rotation_type}")
# Invalidate cache
self._cache_valid = False
@property
def translation(self) -> np.ndarray:
"""
Get translation vector.
Returns:
Translation array - shape (3,)
"""
return self._translation.copy()
@property
def quat_wxyz(self) -> np.ndarray:
"""Get rotation as quaternion in wxyz order (w, x, y, z)"""
return self.to_rotation("quat", "wxyz")
@property
def quat_xyzw(self) -> np.ndarray:
"""Get rotation as quaternion in xyzw order (x, y, z, w)"""
return self.to_rotation("quat", "xyzw")
@property
def euler_xyz(self) -> np.ndarray:
"""Get rotation as Euler angles in xyz order (degrees)"""
return self.to_rotation("euler", "xyz")
@property
def rotvec(self) -> np.ndarray:
"""Get rotation as rotation vector (axis-angle)"""
return self.to_rotation("rotvec")
@property
def rotation_matrix(self) -> np.ndarray:
"""Get rotation as 3x3 rotation matrix"""
return self.to_rotation("matrix")
@property
def rot6d(self) -> np.ndarray:
"""Get rotation as 6D representation (first two rows of rotation matrix)"""
return self.to_rotation("rot6d")
@property
def xyz_rot6d(self) -> np.ndarray:
"""Get pose as concatenated translation and 6D rotation (9,)"""
return np.concatenate([self._translation, self.rot6d])
@property
def xyz_rotvec(self) -> np.ndarray:
"""Get pose as concatenated translation and rotation vector (6,)"""
return np.concatenate([self._translation, self.rotvec])
@property
def homogeneous(self) -> np.ndarray:
"""
Get homogeneous transformation matrix.
Returns:
Homogeneous matrix - shape (4, 4)
"""
if not self._cache_valid:
self._homogeneous_cache = self._compute_homogeneous()
self._cache_valid = True
assert self._homogeneous_cache is not None
return self._homogeneous_cache.copy()
def _compute_homogeneous(self) -> np.ndarray:
"""Compute homogeneous transformation matrix"""
H = np.eye(4)
H[:3, :3] = self._rotation.as_matrix()
H[:3, 3] = self._translation
return H
def to_rotation(
self,
rotation_type: str,
rotation_order: str | None = None,
degrees: bool = True,
) -> np.ndarray:
"""
Get rotation in specified representation.
Args:
rotation_type: Desired type ("quat", "euler", "rotvec", "matrix", "rot6d")
rotation_order: Order/convention for the rotation type
degrees: For Euler angles, return in degrees (default True)
Returns:
Rotation in requested format
- Shape (4,) for quat
- Shape (3,) for euler/rotvec
- Shape (6,) for rot6d
- Shape (3, 3) for matrix
"""
rot_type = RotationType(rotation_type.lower())
if rot_type == RotationType.ROT6D:
rotation_matrix = self._rotation.as_matrix()
return self._matrix_to_rot6d(rotation_matrix)
if rot_type == RotationType.QUAT:
quat_order = QuatOrder(rotation_order.lower()) if rotation_order else QuatOrder.WXYZ
quat_xyzw = self._rotation.as_quat()
if quat_order == QuatOrder.WXYZ:
return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]])
else:
return quat_xyzw
elif rot_type == RotationType.EULER:
euler_order = EulerOrder(rotation_order.lower()) if rotation_order else EulerOrder.XYZ
return self._rotation.as_euler(euler_order.value, degrees=degrees)
elif rot_type == RotationType.ROTVEC:
return self._rotation.as_rotvec()
elif rot_type == RotationType.MATRIX:
return self._rotation.as_matrix()
else:
raise ValueError(f"Unknown rotation type: {rotation_type}")
def to_homogeneous(self) -> np.ndarray:
"""
Convert pose to homogeneous transformation matrix.
(Alias for the homogeneous property)
Returns:
Homogeneous matrix - shape (4, 4)
"""
return self.homogeneous
def set_rotation(
self,
rotation: list | np.ndarray,
rotation_type: str,
rotation_order: str | None = None,
degrees: bool = True,
):
"""
Set rotation from specified representation.
Args:
rotation: Rotation data
rotation_type: Type of rotation ("quat", "euler", "rotvec", "matrix", "rot6d")
rotation_order: Order/convention for the rotation type
degrees: For Euler angles, whether the input is in degrees (default True)
"""
self._set_rotation(rotation, rotation_type, rotation_order, degrees)
def _compute_relative(self, other): # type: ignore[override]
"""
Compute relative transformation from other to self.
The result represents the transformation needed to go from other's frame to self's frame.
Mathematically: T_relative = T_other^{-1} * T_self
Args:
other: Reference end-effector pose
Returns:
EndEffectorPose representing the relative transformation
"""
# Get homogeneous matrices
T_self = self.homogeneous
T_other = other.homogeneous
# Compute relative transformation: T_other^{-1} * T_self
T_relative = relative_transformation(T_other, T_self)
# Create new EndEffectorPose from relative transformation
return EndEffectorPose(homogeneous=T_relative)
@classmethod
def from_action_format(cls, data: np.ndarray, action_format: ActionFormat) -> "EndEffectorPose":
"""
Create an EndEffectorPose from a flat array using the specified action format.
This is the inverse of the xyz_rot6d / xyz_rotvec / homogeneous properties.
Args:
data: Flat array whose layout depends on action_format.
action_format: One of ActionFormat.XYZ_ROT6D, XYZ_ROTVEC, or DEFAULT.
Returns:
EndEffectorPose instance.
"""
if action_format == ActionFormat.XYZ_ROT6D:
return cls(translation=data[:3], rotation=data[3:], rotation_type="rot6d")
elif action_format == ActionFormat.XYZ_ROTVEC:
return cls(translation=data[:3], rotation=data[3:], rotation_type="rotvec")
elif action_format == ActionFormat.DEFAULT:
return cls(homogeneous=data.reshape(4, 4))
else:
raise ValueError(f"Unsupported ActionFormat: {action_format}")
def copy(self) -> "EndEffectorPose":
"""
Create a deep copy of this end-effector pose.
Returns:
New EndEffectorPose instance with copied data
"""
return EndEffectorPose(
translation=self._translation.copy(),
rotation=self._rotation.as_quat(),
rotation_type="quat",
rotation_order="xyzw",
)
def __repr__(self) -> str:
quat = self.to_rotation("quat", "wxyz")
return f"EndEffectorPose(translation={self.translation}, rotation_quat_wxyz={quat})"
def __eq__(self, other) -> bool:
if not isinstance(other, EndEffectorPose):
return False
return np.allclose(self._translation, other._translation) and np.allclose(
self._rotation.as_quat(), other._rotation.as_quat()
)
@@ -0,0 +1,643 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
"""Unified state and action processor for robotics."""
from copy import deepcopy
import numpy as np
from vllm_omni.diffusion.models.gr00t.configs.embodiment_configs import (
ActionFormat,
ActionRepresentation,
ActionType,
ModalityConfig,
)
from vllm_omni.diffusion.models.gr00t.dataio.state_action.action_chunking import (
EndEffectorActionChunk,
JointActionChunk,
)
from vllm_omni.diffusion.models.gr00t.dataio.state_action.pose import EndEffectorPose, JointPose
from vllm_omni.diffusion.models.gr00t.dataio.utils import (
apply_sin_cos_encoding,
nested_dict_to_numpy,
normalize_values_meanstd,
normalize_values_minmax,
parse_modality_configs,
unnormalize_values_meanstd,
unnormalize_values_minmax,
)
class StateActionProcessor:
"""
Unified processor for robot state and action data.
Handles:
- State normalization (min/max, mean/std, sin/cos encoding)
- Action normalization
- Absolute <-> Relative action representation conversion
- Action processing with state dependency
"""
def __init__(
self,
modality_configs: dict[str, dict[str, ModalityConfig]],
statistics: (dict[str, dict[str, dict[str, dict[str, list[float]]]]] | None) = None,
use_percentiles: bool = False,
clip_outliers: bool = True,
apply_sincos_state_encoding: bool = False,
use_relative_action: bool = False,
):
"""
Initialize unified state and action processor.
Args:
modality_configs: Nested dict with structure:
{embodiment_tag: {modality: ModalityConfig}}
where modality in ["state", "action"]
Example: {"gr1": {"state": ModalityConfig(...), "action": ModalityConfig(...)}}
statistics: Optional nested dict with structure:
{embodiment_tag: {modality: {joint_group: {stat_type: values}}}}
where modality in ["state", "action", "relative_action"]
and stat_type in ["min", "max", "mean", "std", "q01", "q99"]
Example: {"gr1": {"state": {"left_arm": {"min": [...], "max": [...], ...}}}}
use_percentiles: Whether to use percentiles (q01/q99) instead of min/max
clip_outliers: Whether to clip normalized values to [-1, 1]
apply_sincos_state_encoding: Global flag to enable sin/cos encoding for states
"""
self.modality_configs = parse_modality_configs(modality_configs)
self.statistics: dict[str, dict[str, dict[str, dict[str, list[float]]]]] = {}
self.use_percentiles = use_percentiles
self.clip_outliers = clip_outliers
self.apply_sincos_state_encoding = apply_sincos_state_encoding
self.use_relative_action = use_relative_action
# Normalization parameters computed from statistics
self.norm_params: dict[str, dict[str, dict[str, dict[str, np.ndarray]]]] = {}
# Format: norm_params[embodiment_tag][modality][joint_group][stat_type]
# where stat_type in ["min", "max", "mean", "std", "dim"]
if statistics is not None:
self.set_statistics(statistics)
def set_statistics(
self,
statistics: dict[str, dict[str, dict[str, dict[str, list[float]]]]],
override: bool = False,
) -> None:
"""
Set dataset statistics for normalization.
Args:
statistics: Nested dict with structure:
{embodiment_tag: {modality: {joint_group: {stat_type: values}}}}
"""
for key in statistics:
if key not in self.statistics or override:
self.statistics[key] = deepcopy(statistics[key])
else:
print(f"Embodiment tag {key} already in statistics, skipping updating")
self._compute_normalization_parameters()
def _compute_normalization_parameters(self) -> None:
"""Compute and cache normalization parameters from statistics for all embodiments and modalities."""
for embodiment_tag in self.statistics:
self.norm_params[embodiment_tag] = {}
for modality in ["state", "action"]:
if modality not in self.statistics[embodiment_tag]:
continue
self.norm_params[embodiment_tag][modality] = {}
for joint_group, stats in self.statistics[embodiment_tag][modality].items():
if self.use_percentiles:
min_vals = np.array(stats["q01"])
max_vals = np.array(stats["q99"])
else:
min_vals = np.array(stats["min"])
max_vals = np.array(stats["max"])
mean_vals = np.array(stats["mean"])
std_vals = np.array(stats["std"])
# Compute range, ensuring it's not zero
range_vals = max_vals - min_vals
range_vals = np.maximum(range_vals, 1e-8)
self.norm_params[embodiment_tag][modality][joint_group] = {
"min": min_vals,
"max": max_vals,
"dim": np.array(range_vals.shape[0]),
"mean": mean_vals,
"std": std_vals,
}
# Override absolute action stats with relative stats where specified
if "action" in self.modality_configs[embodiment_tag]:
modality_keys = self.modality_configs[embodiment_tag]["action"].modality_keys
action_configs = self.modality_configs[embodiment_tag]["action"].action_configs
if action_configs is not None:
for key, action_config in zip(modality_keys, action_configs):
if action_config.rep == ActionRepresentation.RELATIVE and self.use_relative_action:
if "relative_action" not in self.statistics[embodiment_tag]:
raise ValueError(
f"Relative action statistics required for embodiment '{embodiment_tag}' "
f"but 'relative_action' not found in statistics"
)
if key not in self.statistics[embodiment_tag]["relative_action"]:
raise ValueError(
f"Relative action statistics required for key '{key}' "
f"in embodiment '{embodiment_tag}' but not found"
)
action_dim = self.norm_params[embodiment_tag]["action"][key]["dim"]
self.norm_params[embodiment_tag]["action"][key] = nested_dict_to_numpy(
self.statistics[embodiment_tag]["relative_action"][key]
)
self.norm_params[embodiment_tag]["action"][key]["dim"] = action_dim
def apply_state(
self,
state: dict[str, np.ndarray],
embodiment_tag: str,
) -> dict[str, np.ndarray]:
"""
Apply state processing (normalization, encoding).
Args:
state: Dict mapping joint_group -> raw state values
Shape per group: (..., D) where D is state dimension
embodiment_tag: Embodiment identifier (e.g., "gr1")
Returns:
Dict mapping joint_group -> processed state values
- Sin/cos encoded groups: (..., 2*D)
- Other groups: (..., D)
"""
normalized_values = {}
state = deepcopy(state) # Avoid modifying input
# Get sin/cos embedding keys if enabled
sin_cos_keys = None
if self.apply_sincos_state_encoding:
state_config = self.modality_configs[embodiment_tag].get("state")
if state_config and hasattr(state_config, "sin_cos_embedding_keys"):
sin_cos_keys = state_config.sin_cos_embedding_keys
for joint_group in self.modality_configs[embodiment_tag]["state"].modality_keys:
if joint_group not in state:
raise KeyError(f"Joint group '{joint_group}' not found in state dict for embodiment '{embodiment_tag}'")
# Strategy 1: Sin/cos encoding (doubles dimension)
if sin_cos_keys and joint_group in sin_cos_keys:
normalized_values[joint_group] = apply_sin_cos_encoding(state[joint_group])
# Strategy 2: Mean/std normalization
elif (
hasattr(
self.modality_configs[embodiment_tag]["state"],
"mean_std_embedding_keys",
)
and self.modality_configs[embodiment_tag]["state"].mean_std_embedding_keys
and joint_group in self.modality_configs[embodiment_tag]["state"].mean_std_embedding_keys
):
params = self.norm_params[embodiment_tag]["state"][joint_group]
normalized = normalize_values_meanstd(state[joint_group], params)
normalized_values[joint_group] = normalized
# Strategy 3: Min/max normalization to [-1, 1]
else:
params = self.norm_params[embodiment_tag]["state"][joint_group]
normalized = normalize_values_minmax(state[joint_group], params)
if self.clip_outliers:
normalized = np.clip(normalized, -1.0, 1.0)
normalized_values[joint_group] = normalized
return normalized_values
def unapply_state(
self,
state: dict[str, np.ndarray],
embodiment_tag: str,
) -> dict[str, np.ndarray]:
"""
Reverse state processing (denormalization).
Args:
state: Dict mapping joint_group -> processed state values
embodiment_tag: Embodiment identifier
Returns:
Dict mapping joint_group -> raw state values
Raises:
ValueError: If attempting to reverse sin/cos encoding (not reversible)
"""
unnormalized_values = {}
# Get sin/cos embedding keys if enabled
sin_cos_keys = None
if self.apply_sincos_state_encoding:
state_config = self.modality_configs[embodiment_tag].get("state")
if state_config and hasattr(state_config, "sin_cos_embedding_keys"):
sin_cos_keys = state_config.sin_cos_embedding_keys
for joint_group in self.modality_configs[embodiment_tag]["state"].modality_keys:
if joint_group not in state:
raise KeyError(f"Joint group '{joint_group}' not found in state dict for embodiment '{embodiment_tag}'")
# Sin/cos encoding is not reversible
if sin_cos_keys and joint_group in sin_cos_keys:
raise ValueError(
f"Cannot unapply sin/cos encoding for joint group '{joint_group}' "
f"in embodiment '{embodiment_tag}'. This transformation is not reversible."
)
# Reverse mean/std normalization
elif (
hasattr(
self.modality_configs[embodiment_tag]["state"],
"mean_std_embedding_keys",
)
and self.modality_configs[embodiment_tag]["state"].mean_std_embedding_keys
and joint_group in self.modality_configs[embodiment_tag]["state"].mean_std_embedding_keys
):
params = self.norm_params[embodiment_tag]["state"][joint_group]
unnormalized = unnormalize_values_meanstd(state[joint_group], params)
unnormalized_values[joint_group] = unnormalized
# Reverse min/max normalization
else:
params = self.norm_params[embodiment_tag]["state"][joint_group]
unnormalized_values[joint_group] = unnormalize_values_minmax(state[joint_group], params)
return unnormalized_values
def apply_action(
self,
action: dict[str, np.ndarray],
embodiment_tag: str,
state: dict[str, np.ndarray] | None = None,
) -> dict[str, np.ndarray]:
"""
Apply action processing (absolute->relative conversion, normalization).
Processing order:
1. Convert absolute actions to relative (if configured)
2. Normalize actions
Args:
action: Dict mapping joint_group -> raw action values
Shape per group: (T, D) where T is action horizon, D is action dimension
embodiment_tag: Embodiment identifier
state: Optional dict mapping joint_group -> raw state values
Required if any action group uses ActionRepresentation.RELATIVE
Shape per group: (T_state, D) where last timestep is used as reference
Returns:
Dict mapping joint_group -> processed action values
Shape per group: (T, D)
Raises:
ValueError: If state is None but required for relative action conversion
"""
action = deepcopy(action) # Avoid modifying input
# Step 1: Convert absolute actions to relative (if needed)
modality_keys = self.modality_configs[embodiment_tag]["action"].modality_keys
action_configs = self.modality_configs[embodiment_tag]["action"].action_configs
if action_configs is not None:
for key, action_config in zip(modality_keys, action_configs):
if action_config.rep == ActionRepresentation.RELATIVE and self.use_relative_action:
if state is None:
raise ValueError(
f"State dict required for relative action processing of key '{key}' "
f"in embodiment '{embodiment_tag}'"
)
# Determine which state key to use as reference
state_key = action_config.state_key if action_config.state_key else key
if state_key not in state:
raise KeyError(
f"Reference state key '{state_key}' not found in state dict "
f"for embodiment '{embodiment_tag}'"
)
# Use last state as reference frame
reference_state = state[state_key][-1]
# Convert absolute to relative
action[key] = self._convert_to_relative_action(
action=action[key],
reference_state=reference_state,
action_type=action_config.type,
action_format=action_config.format,
)
# Step 2: Normalize actions
normalized_values = {}
for joint_group in modality_keys:
if joint_group not in action:
raise KeyError(
f"Joint group '{joint_group}' not found in action dict for embodiment '{embodiment_tag}'"
)
params = self.norm_params[embodiment_tag]["action"][joint_group]
if (
self.modality_configs[embodiment_tag]["action"].mean_std_embedding_keys is not None
and joint_group in self.modality_configs[embodiment_tag]["action"].mean_std_embedding_keys
):
normalized = normalize_values_meanstd(action[joint_group], params)
else:
normalized = normalize_values_minmax(action[joint_group], params)
if self.clip_outliers:
normalized = np.clip(normalized, -1.0, 1.0)
normalized_values[joint_group] = normalized
return normalized_values
def unapply_action(
self,
action: dict[str, np.ndarray],
embodiment_tag: str,
state: dict[str, np.ndarray] | None = None,
) -> dict[str, np.ndarray]:
"""
Reverse action processing (denormalization, relative->absolute conversion).
Processing order:
1. Denormalize actions
2. Convert relative actions to absolute (if configured)
Args:
action: Dict mapping joint_group -> processed action values
Shape per group: (T, D) or (B, T, D) for batched
embodiment_tag: Embodiment identifier
state: Optional dict mapping joint_group -> raw state values
Required if any action group uses ActionRepresentation.RELATIVE
Shape per group: (T_state, D) or (B, T_state, D) for batched
Returns:
Dict mapping joint_group -> raw absolute action values
Shape per group: (T, D) or (B, T, D) for batched
Raises:
ValueError: If state is None but required for relative->absolute conversion
"""
# Step 1: Unnormalize actions
unnormalized_values = {}
modality_keys = self.modality_configs[embodiment_tag]["action"].modality_keys
for joint_group in modality_keys:
if joint_group not in action:
raise KeyError(
f"Joint group '{joint_group}' not found in action dict for embodiment '{embodiment_tag}'"
)
params = self.norm_params[embodiment_tag]["action"][joint_group]
group_values = action[joint_group]
if (
self.modality_configs[embodiment_tag]["action"].mean_std_embedding_keys is not None
and joint_group in self.modality_configs[embodiment_tag]["action"].mean_std_embedding_keys
):
unnormalized = unnormalize_values_meanstd(group_values, params)
else:
unnormalized = unnormalize_values_minmax(group_values, params)
unnormalized_values[joint_group] = unnormalized
# Step 2: Convert relative actions to absolute (if needed)
action_configs = self.modality_configs[embodiment_tag]["action"].action_configs
if action_configs is not None:
for key, action_config in zip(modality_keys, action_configs):
if action_config.rep == ActionRepresentation.RELATIVE and self.use_relative_action:
if state is None:
raise ValueError(
f"State dict required for relative->absolute conversion of key '{key}' "
f"in embodiment '{embodiment_tag}'"
)
# Determine which state key to use as reference
state_key = action_config.state_key if action_config.state_key else key
if state_key not in state:
raise KeyError(
f"Reference state key '{state_key}' not found in state dict "
f"for embodiment '{embodiment_tag}'"
)
relative_action = unnormalized_values[key]
# Handle batched and unbatched cases
is_batched = relative_action.ndim == 3
if not is_batched:
assert relative_action.ndim == 2
reference_state = state[state_key]
if reference_state.ndim == 2:
reference_state = reference_state[None, :]
relative_action = relative_action[None, :]
else:
reference_state = state[state_key]
if reference_state.ndim == 2:
reference_state = reference_state[None, :]
# Convert batched relative actions to absolute
absolute_actions = []
for s, a in zip(reference_state, relative_action):
# Use last timestep of state as reference
absolute_action = self._convert_to_absolute_action(
action=a,
reference_state=s[-1],
action_type=action_config.type,
action_format=action_config.format,
)
absolute_actions.append(absolute_action)
if is_batched:
unnormalized_values[key] = np.stack(absolute_actions, axis=0)
else:
unnormalized_values[key] = absolute_actions[0]
return unnormalized_values
def apply(
self,
state: dict[str, np.ndarray],
action: dict[str, np.ndarray],
embodiment_tag: str,
) -> tuple[dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
Apply both state and action processing together.
Convenience method that processes state and action in one call,
automatically passing raw state to action processor for relative conversion.
Args:
state: Dict mapping joint_group -> raw state values
action: Dict mapping joint_group -> raw action values
embodiment_tag: Embodiment identifier
Returns:
Tuple of (processed_state, processed_action)
"""
processed_state = self.apply_state(state, embodiment_tag)
if action:
processed_action = self.apply_action(action, embodiment_tag, state=state)
else:
processed_action = {}
return processed_state, processed_action
def unapply(
self,
state: dict[str, np.ndarray],
action: dict[str, np.ndarray],
embodiment_tag: str,
raw_state: dict[str, np.ndarray] | None = None,
) -> tuple[dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
Reverse both state and action processing together.
Args:
state: Dict mapping joint_group -> processed state values
action: Dict mapping joint_group -> processed action values
embodiment_tag: Embodiment identifier
raw_state: Optional dict of raw states for relative->absolute conversion
If None, will use unapplied state (but won't work for sin/cos encoded states)
Returns:
Tuple of (raw_state, raw_action)
"""
# Unapply state first
try:
unapplied_state = self.unapply_state(state, embodiment_tag)
except ValueError as e:
if "sin/cos encoding" in str(e) and raw_state is None:
raise ValueError("Cannot unapply sin/cos encoded state. Please provide raw_state parameter.") from e
raise
# Use provided raw_state if available, otherwise use unapplied state
state_for_action = raw_state if raw_state is not None else unapplied_state
# Unapply action
unapplied_action = self.unapply_action(action, embodiment_tag, state=state_for_action)
return unapplied_state, unapplied_action
def get_state_dim(self, embodiment_tag: str, include_sincos_expansion: bool = False) -> int:
"""
Get total state dimension after processing.
Args:
embodiment_tag: Embodiment identifier
include_sincos_expansion: If True, accounts for sin/cos encoding doubling dimensions
Returns:
Total state dimension across all joint groups
"""
total_dim = 0
state_config = self.modality_configs[embodiment_tag]["state"]
# Get sin/cos embedding keys if enabled
sin_cos_keys = set()
if self.apply_sincos_state_encoding and hasattr(state_config, "sin_cos_embedding_keys"):
sin_cos_keys = set(state_config.sin_cos_embedding_keys)
for joint_group in state_config.modality_keys:
base_dim = self.norm_params[embodiment_tag]["state"][joint_group]["dim"].item()
# Sin/cos encoding doubles the dimension
if include_sincos_expansion and joint_group in sin_cos_keys:
total_dim += base_dim * 2
else:
total_dim += base_dim
return total_dim
def get_action_dim(self, embodiment_tag: str) -> int:
"""
Get total action dimension.
Args:
embodiment_tag: Embodiment identifier
Returns:
Total action dimension across all joint groups
"""
total_dim = 0
for joint_group in self.modality_configs[embodiment_tag]["action"].modality_keys:
total_dim += self.norm_params[embodiment_tag]["action"][joint_group]["dim"].item()
return total_dim
def _convert_to_relative_action(
self,
action: np.ndarray,
reference_state: np.ndarray,
action_type: ActionType,
action_format: ActionFormat,
) -> np.ndarray:
"""Convert absolute action to relative action using reference state."""
assert action.ndim == 2, f"Expected action shape (T, D), got {action.shape}"
assert reference_state.ndim == 1, f"Expected state shape (D,), got {reference_state.shape}"
if action_type == ActionType.EEF:
action_chunking = EndEffectorActionChunk.from_array(action, action_format)
reference_frame = EndEffectorPose.from_action_format(reference_state, action_format)
elif action_type == ActionType.NON_EEF:
action_chunking = JointActionChunk([JointPose(m) for m in action])
reference_frame = JointPose(reference_state)
else:
raise ValueError(f"Unknown ActionType: {action_type}")
relative_action_chunking = action_chunking.relative_chunking(reference_frame=reference_frame)
return relative_action_chunking.to(action_format)
def _convert_to_absolute_action(
self,
action: np.ndarray,
reference_state: np.ndarray,
action_type: ActionType,
action_format: ActionFormat,
) -> np.ndarray:
"""Convert relative action to absolute action using reference state."""
assert action.ndim == 2, f"Expected action shape (T, D), got {action.shape}"
assert reference_state.ndim == 1, f"Expected state shape (D,), got {reference_state.shape}"
assert reference_state.shape[0] == action.shape[1], (
f"State dim {reference_state.shape[0]} != action dim {action.shape[1]}"
)
if action_type == ActionType.EEF:
rel_action = EndEffectorActionChunk.from_array(action, action_format)
reference_frame = EndEffectorPose.from_action_format(reference_state, action_format)
elif action_type == ActionType.NON_EEF:
rel_action = JointActionChunk([JointPose(pose) for pose in action])
reference_frame = JointPose(reference_state)
else:
raise ValueError(f"Unknown ActionType: {action_type}")
abs_action = rel_action.to_absolute_chunking(reference_frame=reference_frame)
return abs_action.to(action_format)
def __str__(self) -> str:
return (
"StateActionProcessor("
f"modality_configs={self.modality_configs}, "
f"statistics={self.statistics}, "
f"use_percentiles={self.use_percentiles}, "
f"clip_outliers={self.clip_outliers}, "
f"apply_sincos_state_encoding={self.apply_sincos_state_encoding}, "
f"use_relative_action={self.use_relative_action})"
)
@@ -0,0 +1,112 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
from dataclasses import dataclass, field
from enum import Enum
from typing import Any
import numpy as np
from vllm_omni.diffusion.models.gr00t.dataio.embodiment_tags import EmbodimentTag
class MessageType(Enum):
START_OF_EPISODE = "start_of_episode"
END_OF_EPISODE = "end_of_episode"
EPISODE_STEP = "episode_step"
IMAGE = "image"
TEXT = "text"
class ActionRepresentation(Enum):
RELATIVE = "relative"
DELTA = "delta"
ABSOLUTE = "absolute"
class ActionType(Enum):
EEF = "eef"
NON_EEF = "non_eef"
class ActionFormat(Enum):
DEFAULT = "default"
XYZ_ROT6D = "xyz+rot6d"
XYZ_ROTVEC = "xyz+rotvec"
@dataclass
class VLAStepData:
"""
Represents a single step of VLA (Vision-Language-Action) data.
This is the core data structure returned by datasets, containing raw observation
and action data that will be processed by the SequenceVLAProcessor.
"""
# Core data
images: dict[str, list[np.ndarray]] # view_name -> list[np.ndarray] (for temporal stacking)
states: dict[str, np.ndarray] # state_name -> np.ndarray (dim,) for single step or (horizon, dim) for trajectory
actions: dict[str, np.ndarray] # action_name -> np.ndarray (horizon, dim) for action chunk
masks: dict[str, list[np.ndarray]] | None = None # view_name -> list[np.ndarray] (H, W)
text: str | None = None # Optional task description or instruction
embodiment: EmbodimentTag = EmbodimentTag.NEW_EMBODIMENT # Optional embodiment tag for cross-embodiment training
is_demonstration: bool = (
False # Whether the step is a demonstration. If True, no loss should be computed for this step.
)
# Flexible metadata that can be extended by users
metadata: dict[str, Any] = field(default_factory=dict)
@dataclass
class ActionConfig:
rep: ActionRepresentation
type: ActionType
format: ActionFormat
state_key: str | None = None
@dataclass
class ModalityConfig:
"""Configuration for a modality defining how data should be sampled and loaded.
This class specifies which indices to sample relative to a base index and which
keys to load for a particular modality (e.g., video, state, action).
"""
delta_indices: list[int]
"""Delta indices to sample relative to the current index. The returned data will
correspond to the original data at a sampled base index + delta indices."""
modality_keys: list[str]
"""The keys to load for the modality in the dataset."""
sin_cos_embedding_keys: list[str] | None = None
"""Optional list of keys to apply sin/cos encoding. If None or empty, use
min/max normalization for all keys."""
mean_std_embedding_keys: list[str] | None = None
"""Optional list of keys to apply mean/std normalization. If None or empty,
use min/max normalization for all keys."""
action_configs: list[ActionConfig] | None = None
def __post_init__(self):
"""Validate fields and set default values."""
if self.delta_indices is None or not isinstance(self.delta_indices, list):
raise ValueError(f"delta_indices must be a non-None list, got {self.delta_indices!r}")
if self.modality_keys is None or not isinstance(self.modality_keys, list) or len(self.modality_keys) == 0:
raise ValueError(f"modality_keys must be a non-empty list, got {self.modality_keys!r}")
if self.action_configs is not None:
assert len(self.action_configs) == len(self.modality_keys), (
f"Number of action configs ({len(self.action_configs)}) must match "
f"number of modality keys ({len(self.modality_keys)})"
)
parsed_action_configs = []
for action_config in self.action_configs:
if isinstance(action_config, dict):
action_config = ActionConfig(
rep=ActionRepresentation[action_config["rep"]],
type=ActionType[action_config["type"]],
format=ActionFormat[action_config["format"]],
state_key=action_config.get("state_key", None),
)
parsed_action_configs.append(action_config)
self.action_configs = parsed_action_configs
@@ -0,0 +1,187 @@
# SPDX-License-Identifier: Apache-2.0
# SPDX-FileCopyrightText: Copyright contributors to the vLLM project
from dataclasses import asdict, is_dataclass
from enum import Enum
from typing import Any
import numpy as np
from vllm_omni.diffusion.models.gr00t.configs.embodiment_configs import ModalityConfig
def apply_sin_cos_encoding(values: np.ndarray) -> np.ndarray:
"""Apply sin/cos encoding to values.
Args:
values: Array of shape (..., D) containing values to encode
Returns:
Array of shape (..., 2*D) with [sin, cos] concatenated
Note: This DOUBLES the dimension. For example:
Input: [v₁, v₂, v₃] with shape (..., 3)
Output: [sin(v₁), sin(v₂), sin(v₃), cos(v₁), cos(v₂), cos(v₃)] with shape (..., 6)
"""
sin_values = np.sin(values)
cos_values = np.cos(values)
# Concatenate sin and cos: [sin(v1), sin(v2), ..., cos(v1), cos(v2), ...]
return np.concatenate([sin_values, cos_values], axis=-1)
def nested_dict_to_numpy(data: Any) -> Any:
"""Recursively convert bottom-level list-of-lists to NumPy arrays.
Args:
data: A nested dictionary whose leaves are lists (or lists of lists).
Returns:
The same dictionary structure with leaf lists converted to ``np.ndarray``.
"""
if isinstance(data, dict):
return {key: nested_dict_to_numpy(value) for key, value in data.items()}
elif isinstance(data, list):
# Convert lists to numpy arrays
# NumPy will handle both 1D and 2D cases appropriately
return np.array(data)
else:
return data
def normalize_values_minmax(values: np.ndarray, params: dict[str, np.ndarray]) -> np.ndarray:
"""Min-max normalize ``values`` to ``[-1, 1]`` using ``params['min']`` and ``params['max']``.
Accepts 2D ``(T, D)`` or 3D ``(B, T, D)`` arrays. Features with ``min == max``
are emitted as 0.
"""
min_vals = params["min"]
max_vals = params["max"]
normalized = np.zeros_like(values)
mask = ~np.isclose(max_vals, min_vals)
normalized[..., mask] = (values[..., mask] - min_vals[..., mask]) / (max_vals[..., mask] - min_vals[..., mask])
normalized[..., mask] = 2 * normalized[..., mask] - 1
return normalized
def unnormalize_values_minmax(normalized_values: np.ndarray, params: dict[str, np.ndarray]) -> np.ndarray:
"""Inverse of :func:`normalize_values_minmax`.
Input values are clipped to ``[-1, 1]`` before being mapped back to
``[params['min'], params['max']]``.
"""
min_vals = params["min"]
max_vals = params["max"]
range_vals = max_vals - min_vals
# Unnormalize from [-1, 1]
unnormalized = (np.clip(normalized_values, -1.0, 1.0) + 1.0) / 2.0 * range_vals + min_vals
return unnormalized
def normalize_values_meanstd(values: np.ndarray, params: dict[str, np.ndarray]) -> np.ndarray:
"""Z-score normalize ``values`` using ``params['mean']`` and ``params['std']``.
Features whose ``std == 0`` are passed through unchanged.
"""
mean_vals = params["mean"]
std_vals = params["std"]
# Create mask for non-zero standard deviations
mask = std_vals != 0
# Initialize normalized array
normalized = np.zeros_like(values)
# Normalize only features with non-zero std
normalized[..., mask] = (values[..., mask] - mean_vals[..., mask]) / std_vals[..., mask]
# Keep original values for zero-std features
normalized[..., ~mask] = values[..., ~mask]
return normalized
def unnormalize_values_meanstd(normalized_values: np.ndarray, params: dict[str, np.ndarray]) -> np.ndarray:
"""Inverse of :func:`normalize_values_meanstd` (``x * std + mean``).
Features whose ``std == 0`` are passed through unchanged.
"""
mean_vals = params["mean"]
std_vals = params["std"]
# Create mask for non-zero standard deviations
mask = std_vals != 0
# Initialize unnormalized array
unnormalized = np.zeros_like(normalized_values)
# Unnormalize only features with non-zero std
unnormalized[..., mask] = normalized_values[..., mask] * std_vals[..., mask] + mean_vals[..., mask]
# Keep normalized values for zero-std features
unnormalized[..., ~mask] = normalized_values[..., ~mask]
return unnormalized
def to_json_serializable(obj: Any) -> Any:
"""
Recursively convert dataclasses and numpy arrays to JSON-serializable format.
Args:
obj: Object to convert (can be dataclass, numpy array, dict, list, etc.)
Returns:
JSON-serializable representation of the object
"""
if is_dataclass(obj) and not isinstance(obj, type):
# Convert dataclass to dict, then recursively process the dict
return to_json_serializable(asdict(obj))
elif isinstance(obj, np.ndarray):
# Convert numpy array to list
return obj.tolist()
elif isinstance(obj, np.integer):
# Convert numpy integers to Python int
return int(obj)
elif isinstance(obj, np.floating):
# Convert numpy floats to Python float
return float(obj)
elif isinstance(obj, np.bool_):
# Convert numpy bool to Python bool
return bool(obj)
elif isinstance(obj, dict):
# Recursively process dictionary values
return {key: to_json_serializable(value) for key, value in obj.items()}
elif isinstance(obj, (list, tuple)):
# Recursively process list/tuple elements
return [to_json_serializable(item) for item in obj]
elif isinstance(obj, set):
# Convert set to list
return [to_json_serializable(item) for item in obj]
elif isinstance(obj, (str, int, float, bool, type(None))):
# Already JSON-serializable
return obj
elif isinstance(obj, Enum):
return obj.name
else:
# For other types, try to convert to string as fallback
# You might want to handle specific types differently
return str(obj)
def parse_modality_configs(
modality_configs: dict[str, dict[str, ModalityConfig]],
) -> dict[str, dict[str, ModalityConfig]]:
parsed_modality_configs = {}
for embodiment_tag, modality_config in modality_configs.items():
parsed_modality_configs[embodiment_tag] = {}
for modality, config in modality_config.items():
if isinstance(config, dict):
parsed_modality_configs[embodiment_tag][modality] = ModalityConfig(**config)
else:
parsed_modality_configs[embodiment_tag][modality] = config
return parsed_modality_configs