Pose

Pose 是 ManiSkill 对 sapien.Pose 的批量封装，统一了 CPU/GPU 与多并行子场景下的位姿管理，支持张量/列表/SAPIEN 对象互转，并提供与 SAPIEN 相同的乘法、求逆、矩阵转换等接口。

创建方式

• Pose.create_from_pq(p=None, q=None, device=None)：从位置 p 与四元数 q 构造，可自动补零/单位四元数、推断设备，并在批尺寸不一致时自动广播。
• Pose.create(pose, device=None)：接受 sapien.Pose、Pose、(N,7)/(7,) 张量或 sapien.Pose 列表，统一转成批量存储。若输入仅含位置 (N,3)，会自动调用 create_from_pq。
• 索引/切片：pose[0]、pose[1:3] 返回新的 Pose，保持批语义；len(pose) 等于批大小。
• Pose.to(device)：迁移到指定 torch.device。

    def __getitem__(self, i):
        return Pose.create(self.raw_pose[i])

    def to(self, device: Device):
        if self.raw_pose.device == device:
            return self
        return Pose.create(self.raw_pose.to(device))

属性

• raw_pose: (N,7) 张量，前 3 维为位置 p，后 4 维为单位四元数 q。
• p / q: 读写属性，直接返回 (N,3)、(N,4)；可用 pose.p = ...、pose.q = ... 原地更新。
• shape: torch.Size(N,7)；device: 所在设备。
• sp: 将批量 size 必须为 1 的 Pose 转回 sapien.Pose。

主要方法

• __mul__(other): 批量实现 SAPIEN Pose 乘法，自动广播批维。
• inv(): 返回批量求逆的 Pose。
• to_transformation_matrix(): 生成 (N,4,4) 齐次变换矩阵。
• get_p() / get_q(), set_p(p) / set_q(q): PyTorch 风格 getter/setter。

辅助函数

• vectorize_pose(pose, device=None): 将 sapien.Pose/Pose/原始数组转换成 (N,7) 张量。
• to_sapien_pose(pose): 将单个 Pose 或 (7,) 向量转成 sapien.Pose（批量输入需先索引）。

def vectorize_pose(...):
    ...
def to_sapien_pose(pose):
    ...

示例

from mani_skill.utils.structs.pose import Pose

# 直接用位置和四元数构造，并广播四元数
pose = Pose.create_from_pq(
    p=[[0, 0, 0.1], [0.1, 0, 0.1]],
    q=[1, 0, 0, 0],
)

# 乘法、求逆与矩阵化操作
tcp_in_world = Pose.create(env.agent.tcp_pose.raw_pose)
cube_in_world = Pose.create(env.cube.pose.raw_pose)
tcp_to_cube = tcp_in_world.inv() * cube_in_world
transform = tcp_to_cube.to_transformation_matrix()   # shape = (1,4,4)