2. 设计跨具身统一动作空间

任务目标

设计一个跨具身统一动作空间，使单臂机器人、双臂机器人和轮式机器人能够共享同一套动作表示协议，从而为后续多任务VLA模型的联合训练提供统一的输出接口。

核心设计思路

不同机器人的动作差异主要体现在：执行末端数量（单臂1个、双臂2个）、运动模式（机械臂关节运动 vs 轮式底盘平移+旋转）、以及末端执行器状态（夹爪开合）。统一动作空间采用“末端位姿增量 + 执行器状态 + 运动模式标志”的抽象表示，将异构动作映射为同构张量。

统一动作空间定义

# unified_action_space.py
import numpy as np
from dataclasses import dataclass
from typing import List, Optional

# 运动模式枚举
MODE_ARM = 0       # 机械臂末端笛卡尔增量控制
MODE_BASE = 1      # 轮式底盘速度控制

# 末端执行器状态
GRIPPER_OPEN = 0
GRIPPER_CLOSE = 1

@dataclass
class UnifiedAction:
    mode: int                    # MODE_ARM 或 MODE_BASE
    # 末端位姿增量（平移+旋转），最多支持2个末端
    # 每个末端6维: [dx, dy, dz, droll, dpitch, dyaw]
    # 未使用的末端填充0
    end_effectors: np.ndarray    # shape: (2, 6), float32
    gripper_states: np.ndarray   # shape: (2,), int (0/1)
    # 轮式底盘线速度与角速度（仅mode=MODE_BASE时有效）
    linear_vel: float = 0.0      # m/s
    angular_vel: float = 0.0     # rad/s

    def to_tensor(self) -> np.ndarray:
        """转换为固定长度向量，供VLA模型输出"""
        # 维度: mode(1) + end_effectors(12) + gripper(2) + linear(1) + angular(1) = 17
        tensor = np.zeros(17, dtype=np.float32)
        tensor[0] = self.mode
        tensor[1:13] = self.end_effectors.flatten()
        tensor[13:15] = self.gripper_states
        tensor[15] = self.linear_vel
        tensor[16] = self.angular_vel
        return tensor

    @staticmethod
    def from_tensor(tensor: np.ndarray) -> 'UnifiedAction':
        """从模型输出张量还原为动作对象"""
        assert tensor.shape[0] == 17, f"期望17维，实际{tensor.shape[0]}维"
        mode = int(round(tensor[0]))
        ee = tensor[1:13].reshape(2, 6)
        gripper = np.round(tensor[13:15]).astype(int)
        return UnifiedAction(
            mode=mode,
            end_effectors=ee,
            gripper_states=gripper,
            linear_vel=float(tensor[15]),
            angular_vel=float(tensor[16])
        )

# ========== 各机器人动作 → 统一动作 转换器 ==========

def single_arm_to_unified(joint_delta: np.ndarray, gripper_cmd: int) -> UnifiedAction:
    """
    单臂机器人（如Franka）动作转换。
    输入: joint_delta为7维关节角增量（弧度），此处简化为直接映射到末端笛卡尔增量。
    实际工程中需通过机器人正运动学Jacobian将关节增量转为末端位姿增量。
    """
    # 简化演示：假设关节增量前6维近似对应末端6维笛卡尔增量
    ee_delta = np.zeros((2, 6), dtype=np.float32)
    ee_delta[0] = joint_delta[:6]  # 取前6维作为末端增量
    gripper = np.array([gripper_cmd, GRIPPER_OPEN], dtype=np.int32)
    return UnifiedAction(mode=MODE_ARM, end_effectors=ee_delta, gripper_states=gripper)

def dual_arm_to_unified(left_ee_delta: np.ndarray, right_ee_delta: np.ndarray,
                        left_gripper: int, right_gripper: int) -> UnifiedAction:
    """双臂机器人（如ALOHA）动作转换。每个末端6维增量"""
    ee = np.stack([left_ee_delta, right_ee_delta]).astype(np.float32)
    gripper = np.array([left_gripper, right_gripper], dtype=np.int32)
    return UnifiedAction(mode=MODE_ARM, end_effectors=ee, gripper_states=gripper)

def wheeled_robot_to_unified(linear: float, angular: float) -> UnifiedAction:
    """轮式底盘（如Jackal）动作转换"""
    return UnifiedAction(
        mode=MODE_BASE,
        end_effectors=np.zeros((2, 6), dtype=np.float32),
        gripper_states=np.zeros(2, dtype=np.int32),
        linear_vel=linear,
        angular_vel=angular
    )

# ========== 验证示例 ==========
if __name__ == "__main__":
    # 1. 单臂动作转换与还原
    arm_action = single_arm_to_unified(
        joint_delta=np.array([0.1, -0.05, 0.02, 0.0, 0.0, 0.3, 0.0]),
        gripper_cmd=GRIPPER_CLOSE
    )
    tensor = arm_action.to_tensor()
    restored = UnifiedAction.from_tensor(tensor)
    assert restored.mode == MODE_ARM
    assert np.allclose(restored.end_effectors[0], arm_action.end_effectors[0])
    assert restored.gripper_states[0] == GRIPPER_CLOSE
    print(f"单臂动作张量维度: {tensor.shape[0]}")

    # 2. 双臂动作转换
    dual_action = dual_arm_to_unified(
        left_ee_delta=np.array([0.1, 0, 0, 0, 0, 0]),
        right_ee_delta=np.array([-0.1, 0, 0, 0, 0, 0]),
        left_gripper=GRIPPER_OPEN,
        right_gripper=GRIPPER_CLOSE
    )
    tensor2 = dual_action.to_tensor()
    restored2 = UnifiedAction.from_tensor(tensor2)
    assert restored2.end_effectors.shape == (2, 6)
    assert restored2.gripper_states[1] == GRIPPER_CLOSE
    print(f"双臂动作张量维度: {tensor2.shape[0]}")

    # 3. 轮式动作转换
    base_action = wheeled_robot_to_unified(linear=0.5, angular=0.2)
    tensor3 = base_action.to_tensor()
    restored3 = UnifiedAction.from_tensor(tensor3)
    assert restored3.mode == MODE_BASE
    assert abs(restored3.linear_vel - 0.5) < 1e-6
    print(f"轮式动作张量维度: {tensor3.shape[0]}")

    print("全部转换与还原验证通过")

运行与验证

将上述代码保存为 unified_action_space.py
运行：python unified_action_space.py
预期输出：

单臂动作张量维度: 17
双臂动作张量维度: 17
轮式动作张量维度: 17
全部转换与还原验证通过

通过条件：程序无异常退出，且打印上述4行输出。这验证了三种异构机器人动作均可映射为17维统一张量，并能无损还原。

关键说明

维度设计：17维 = 1(mode) + 12(2×6末端增量) + 2(夹爪) + 1(线速度) + 1(角速度)。单臂和双臂机器人的末端增量维度统一为2×6，未使用的末端置零。
模式区分：mode字段决定解释方式——MODE_ARM时关注末端增量与夹爪，MODE_BASE时关注线速度与角速度。
实际工程接入：单臂机器人的关节增量到末端笛卡尔增量的转换需通过机器人运动学Jacobian实现，本案例为聚焦统一动作空间设计，使用简化映射演示接口协议。
扩展性：若需支持更多末端（如四足或人形），只需增加end_effectors第一维度并同步调整to_tensor中的索引偏移。

📚 课程目录