import numpy as np import os import cv2 from XPolicyLab.utils.load_file import load_json, load_yaml def _validate_config(action_type: str, robot_action_dim_info: dict, source_type: str): """ Validate configuration and return normalized values. Args: action_type: 'joint' or 'ee'. robot_action_dim_info: Dict with keys: - 'arm_dim': list[int] - 'ee_dim': list[int] source_type: 'obs' or 'dataset'. Returns: arm_dims, ee_dims, num_arms """ if action_type not in {"joint", "ee"}: raise ValueError( f"Unsupported action_type: {action_type!r}. " "Supported values are 'joint' and 'ee'." ) if source_type not in {"obs", "dataset"}: raise ValueError( f"Unsupported source_type: {source_type!r}. " "Supported values are 'obs' and 'dataset'." ) if "arm_dim" not in robot_action_dim_info or "ee_dim" not in robot_action_dim_info: raise KeyError("robot_action_dim_info must contain both 'arm_dim' and 'ee_dim'.") arm_dims = robot_action_dim_info["arm_dim"] ee_dims = robot_action_dim_info["ee_dim"] if not isinstance(arm_dims, (list, tuple)) or not isinstance(ee_dims, (list, tuple)): raise TypeError("'arm_dim' and 'ee_dim' must be list or tuple.") if len(arm_dims) != len(ee_dims): raise ValueError( f"'arm_dim' and 'ee_dim' must have the same length, " f"got {len(arm_dims)} and {len(ee_dims)}." ) if len(arm_dims) not in {1, 2}: raise ValueError( f"Only single-arm or dual-arm robots are supported, got {len(arm_dims)} arms." ) if any(d <= 0 for d in arm_dims) or any(d <= 0 for d in ee_dims): raise ValueError("All dimensions in 'arm_dim' and 'ee_dim' must be positive.") return list(arm_dims), list(ee_dims), len(arm_dims) def _get_state_keys(action_type: str, num_arms: int, source_type: str): """ Return arm keys and ee keys for the current state schema. source_type='obs' uses singular keys: single-arm: action_type='joint' -> ['joint_state'], ['ee_joint_state'] action_type='ee' -> ['ee_pose'], ['ee_joint_state'] dual-arm: action_type='joint' -> ['left_arm_joint_state', 'right_arm_joint_state'], ['left_ee_joint_state', 'right_ee_joint_state'] action_type='ee' -> ['left_ee_pose', 'right_ee_pose'], ['left_ee_joint_state', 'right_ee_joint_state'] source_type='dataset' uses plural keys: single-arm: action_type='joint' -> ['joint_states'], ['ee_joint_states'] action_type='ee' -> ['ee_poses'], ['ee_joint_states'] dual-arm: action_type='joint' -> ['left_arm_joint_states', 'right_arm_joint_states'], ['left_ee_joint_states', 'right_ee_joint_states'] action_type='ee' -> ['left_ee_poses', 'right_ee_poses'], ['left_ee_joint_states', 'right_ee_joint_states'] """ suffix = "" if source_type == "obs" else "s" if num_arms == 1: arm_keys = [f"joint_state{suffix}"] if action_type == "joint" else [f"ee_pose{suffix}"] ee_keys = [f"ee_joint_state{suffix}"] else: if action_type == "joint": arm_keys = [ f"left_arm_joint_state{suffix}", f"right_arm_joint_state{suffix}", ] else: arm_keys = [ f"left_ee_pose{suffix}", f"right_ee_pose{suffix}", ] ee_keys = [ f"left_ee_joint_state{suffix}", f"right_ee_joint_state{suffix}", ] return arm_keys, ee_keys def _ensure_valid_state_array(name: str, value, expected_last_dim: int) -> np.ndarray: """Convert value to np.ndarray and validate only the last dimension.""" arr = np.asarray(value) if arr.shape[-1] != expected_last_dim: raise ValueError( f"State field '{name}' last dim mismatch: " f"expected {expected_last_dim}, got {arr.shape[-1]}." ) return arr def pack_robot_state( obs: dict, action_type: str, robot_action_dim_info: dict, source_type: str = "obs", state_type: str = "state", ) -> np.ndarray: """ Pack robot state from obs['state'] into one vector along the last dimension. Packing order: single-arm: [arm_0, ee_0] dual-arm: [arm_0, ee_0, arm_1, ee_1] """ if state_type not in obs: raise KeyError(f"Input obs must contain a '{state_type}' field.") state_dict = obs[state_type] arm_dims, ee_dims, num_arms = _validate_config(action_type, robot_action_dim_info, source_type) arm_keys, ee_keys = _get_state_keys(action_type, num_arms, source_type) parts = [] expected_prefix_shape = None for i, (arm_key, ee_key, arm_dim, ee_dim) in enumerate( zip(arm_keys, ee_keys, arm_dims, ee_dims) ): if arm_key not in state_dict: raise KeyError(f"Missing key '{arm_key}' in obs['state'] for arm {i}.") if ee_key not in state_dict: raise KeyError(f"Missing key '{ee_key}' in obs['state'] for arm {i}.") arm_value = _ensure_valid_state_array(arm_key, state_dict[arm_key], arm_dim) ee_value = _ensure_valid_state_array(ee_key, state_dict[ee_key], ee_dim) if arm_value.shape[:-1] != ee_value.shape[:-1]: raise ValueError( f"'{arm_key}' and '{ee_key}' must share the same prefix shape, " f"got {arm_value.shape[:-1]} and {ee_value.shape[:-1]}." ) if expected_prefix_shape is None: expected_prefix_shape = arm_value.shape[:-1] elif arm_value.shape[:-1] != expected_prefix_shape: raise ValueError( "All state fields must share the same prefix shape. " f"Expected {expected_prefix_shape}, got {arm_value.shape[:-1]} for '{arm_key}'." ) parts.append(np.concatenate([arm_value, ee_value], axis=-1)) return np.concatenate(parts, axis=-1) def unpack_robot_state( packed_state, action_type: str, robot_action_dim_info: dict, source_type: str = "obs", ): """ Unpack packed robot state. Rules: - source_type='obs': * ndim must be <= 2 * if ndim == 1: return dict * if ndim == 2: return list[dict] - source_type='dataset': * return dict of arrays directly Unpacking order: single-arm: [arm_0, ee_0] dual-arm: [arm_0, ee_0, arm_1, ee_1] """ arm_dims, ee_dims, num_arms = _validate_config(action_type, robot_action_dim_info, source_type) arm_keys, ee_keys = _get_state_keys(action_type, num_arms, source_type) packed = np.asarray(packed_state) expected_dim = sum(arm_dims) + sum(ee_dims) if packed.shape[-1] != expected_dim: raise ValueError( f"packed_state last dim mismatch: expected {expected_dim}, got {packed.shape[-1]}." ) if source_type == "obs": assert packed.ndim <= 2, ( f"When source_type='obs', packed_state.ndim must be <= 2, got {packed.ndim}." ) def _unpack_single_action(single_action: np.ndarray) -> dict: result = {} offset = 0 for arm_key, ee_key, arm_dim, ee_dim in zip( arm_keys, ee_keys, arm_dims, ee_dims ): result[arm_key] = single_action[offset : offset + arm_dim] offset += arm_dim result[ee_key] = single_action[offset : offset + ee_dim] offset += ee_dim return result if packed.ndim == 1: return _unpack_single_action(packed) return [_unpack_single_action(single_action) for single_action in packed] result = {} offset = 0 for arm_key, ee_key, arm_dim, ee_dim in zip(arm_keys, ee_keys, arm_dims, ee_dims): result[arm_key] = packed[..., offset : offset + arm_dim] offset += arm_dim result[ee_key] = packed[..., offset : offset + ee_dim] offset += ee_dim return result def get_robot_action_dim_info(env_cfg_type): env_cfg = load_yaml(os.path.join(os.path.dirname(__file__), "../../env_cfg", f"{env_cfg_type}.yml")) robot_name = env_cfg['config']['robot'] robot_action_dim_info = load_json(os.path.join(os.path.dirname(__file__), "../../env_cfg/robot", "_robot_info.json"))[robot_name] return robot_action_dim_info def get_batch_size(env_cfg_type): env_cfg = load_yaml(os.path.join(os.path.dirname(__file__), "../../env_cfg", f"{env_cfg_type}.yml")) sim_cfg = env_cfg['config']['sim'] sim_info = load_yaml(os.path.join(os.path.dirname(__file__), "../../env_cfg/sim", f"{sim_cfg}.yml")) return sim_info['scene']['num_envs'] def get_action_dim(env_cfg_type): env_cfg = load_yaml(os.path.join(os.path.dirname(__file__), "../../env_cfg", f"{env_cfg_type}.yml")) robot_name = env_cfg['config']['robot'] robot_action_dim_info = load_json(os.path.join(os.path.dirname(__file__), "../../env_cfg/robot", "_robot_info.json"))[robot_name] return sum(robot_action_dim_info["arm_dim"]) + sum(robot_action_dim_info["ee_dim"]) def _decode_single_image_bit(image_bit): """Decode one encoded image buffer into an HWC uint8 RGB array.""" if isinstance(image_bit, np.ndarray) and image_bit.dtype.kind in {"S", "U"}: image_bit = image_bit.item() if image_bit.ndim == 0 else image_bit.tobytes() if isinstance(image_bit, str): image_bit = image_bit.encode("utf-8") elif isinstance(image_bit, memoryview): image_bit = image_bit.tobytes() if isinstance(image_bit, (bytes, bytearray)): # Fixed-width HDF5 byte columns pad the tail with NUL. image_bit = image_bit.rstrip(b"\0") elif isinstance(image_bit, np.ndarray): image_bit = np.ascontiguousarray(image_bit) # The returned array is RGB. This is the conclusion for this repo — do not # "correct" it with the usual "cv2 means BGR" rule. cv2.imencode/imdecode # only carry channels in the order they were handed in, so the round trip # is an identity on channel order, and XPolicyLab trajectory files and # runtime observations are encoded from RGB arrays. Adding a COLOR_BGR2RGB # swap here (or in any caller) is what actually breaks the channel order. image = cv2.imdecode(np.frombuffer(image_bit, np.uint8), cv2.IMREAD_COLOR) if image is None: raise ValueError( f"Failed to decode image bits (type={type(image_bit).__name__}, " f"size={getattr(image_bit, 'size', len(image_bit) if hasattr(image_bit, '__len__') else '?')})." ) return image def _decode_image_bit_sequence(image_bits): frames = [] for index, image_bit in enumerate(image_bits): try: frames.append(decode_image_bit(image_bit)) except ValueError as exc: raise ValueError(f"Frame {index}: {exc}") from exc if not frames: return np.zeros((0,), dtype=np.uint8) return np.stack(frames, axis=0) def decode_image_bit(image_bits): """ Decode encoded image bit stream(s) into uint8 RGB image array(s). The output is RGB. Treat that as settled and do not apply the usual "OpenCV returns BGR" rule: XPolicyLab trajectory files and runtime observations store buffers encoded from RGB arrays, and encode/decode round trips preserve the channel order they were given. Never add a COLOR_BGR2RGB after this function to "correct" the output — there is nothing to correct, and the swap is what breaks the channel order. Deliberately converting the RGB result to BGR is a different thing and is allowed where a checkpoint was trained on BGR data; that must be an opt-in documented in the adapter (see Dexora_1B's `input_color_order`), never a silent fix applied at the decode site. Values that are already decoded are returned unchanged, so this function is safe to call on an observation or trajectory field without knowing whether the producer encoded it. Dispatch is on dtype first, then ndim: - bytes / bytearray / memoryview / str -> one encoded buffer - ndarray of dtype kind 'S', 'U', 'O' -> sequence of encoded buffers, or one buffer when 0-d - uint8 ndarray, ndim == 1 -> one encoded buffer - uint8 ndarray, ndim == 2 -> (T, N) stack of encoded buffers - uint8 ndarray, ndim >= 3 -> already decoded, returned as is - ndarray of any other dtype -> already decoded, returned as is - list / tuple -> element-wise, stacked on axis 0 Grayscale (H, W) uint8 images are not supported: a 2-D uint8 array is always read as a stack of encoded buffers. Raises: ValueError: if any buffer fails to decode. """ if isinstance(image_bits, (bytes, bytearray, memoryview, str)): return _decode_single_image_bit(image_bits) if isinstance(image_bits, np.ndarray): if image_bits.dtype.kind in {"S", "U", "O"}: if image_bits.ndim == 0: return _decode_single_image_bit(image_bits.item()) return _decode_image_bit_sequence(image_bits) if image_bits.dtype == np.uint8: if image_bits.ndim == 1: return _decode_single_image_bit(image_bits) if image_bits.ndim == 2: return _decode_image_bit_sequence(image_bits) return image_bits if isinstance(image_bits, (list, tuple)): return _decode_image_bit_sequence(image_bits) return _decode_single_image_bit(image_bits) # Color fields of one camera in an observation. 'depth' is deliberately absent: # depth is stored as 16-bit or float data that cv2.IMREAD_COLOR would destroy. OBS_IMAGE_KEYS = ("color", "colors", "rgb", "image") _DECODABLE_TYPES = (bytes, bytearray, memoryview, np.ndarray, list, tuple) def _decode_obs_image(value, camera_name, image_key): if value is None: return value try: return decode_image_bit(value) except (ValueError, TypeError) as exc: raise ValueError( f"Failed to decode obs['vision']['{camera_name}']['{image_key}']: {exc}" ) from exc def decode_obs_images(obs): """ Decode the encoded color streams of a runtime observation, in place. The policy server calls this before handing an observation to the model, so `update_obs` / `update_obs_batch` always receive plain RGB image arrays and no adapter has to decode anything. Values that are already decoded pass through untouched, and only the color fields listed in OBS_IMAGE_KEYS are visited — depth maps, intrinsics, extrinsics and shapes are left alone. What the model receives is RGB. That is the conclusion; `model.py` must not swap channels to "undo" an OpenCV BGR convention that does not apply here. Args: obs: One observation dict, or a list/tuple of them for batched eval. Anything else is returned unchanged. Returns: The same object, with encoded color fields replaced by decoded arrays. Raises: ValueError: if a color field fails to decode, naming the camera. """ if isinstance(obs, (list, tuple)): for single_obs in obs: decode_obs_images(single_obs) return obs if not isinstance(obs, dict): return obs vision = obs.get("vision") if not isinstance(vision, dict): return obs for camera_name, camera in vision.items(): if isinstance(camera, dict): for image_key in OBS_IMAGE_KEYS: if image_key in camera: camera[image_key] = _decode_obs_image( camera[image_key], camera_name, image_key ) elif isinstance(camera, _DECODABLE_TYPES): # Layout where vision/ is the image itself. vision[camera_name] = _decode_obs_image(camera, camera_name, "color") return obs def images_encoding(imgs): """ JPEG-encode frames for storage in the XPolicyLab layout, padded to a common length so they fit a fixed-width HDF5 byte column. Feed this RGB, never BGR and never a channel-swapped array: cv2.imencode writes channels in the order it is given, so whatever goes in is what `decode_image_bit` hands back, and the rest of the pipeline assumes RGB. """ encode_data = [] padded_data = [] max_len = 0 for i in range(len(imgs)): success, encoded_image = cv2.imencode(".jpg", imgs[i]) jpeg_data = encoded_image.tobytes() encode_data.append(jpeg_data) max_len = max(max_len, len(jpeg_data)) # padding for i in range(len(imgs)): padded_data.append(encode_data[i].ljust(max_len, b"\0")) return encode_data, max_len