[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:
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
@@ -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
|
||||
Reference in new issue
Block a user