具身大模型与VLA：3D-VLA空间推理

import numpy as npfrom dataclasses import dataclassfrom typing import List, Tuple @dataclassclass Object3D:    """三维空间中的物体"""    name: str    position: np.ndarray  # 3D坐标 [x, y, z]    size: np.ndarray     # 物体尺寸 [w, h, d] @dataclassclass GraspPose:    """抓取位姿（简化为2D平面问题）"""    x: float    y: float    angle: float class VLA3DSpatialReasoner:    """最简版3D-VLA空间推理演示"""        def __init__(self):        self.scene_objects: List[Object3D] = []        self.arm_position = np.array([0.0, 0.0, 0.0])        self.language_map = {            "酱油瓶": "soy_sauce",            "盐罐": "salt_shaker",            "油壶": "oil_bottle"        }        def add_object(self, obj: Object3D):        """添加物体到场景"""        self.scene_objects.append(obj)        def find_object(self, lang_instruction: str) -> Tuple[Object3D, str]:        """语言指令 → 目标物体定位"""        target_key = self.language_map[lang_instruction]        for obj in self.scene_objects:            if obj.name == target_key:                return obj, lang_instruction        raise ValueError(f"场景中未找到指令对应的物体: {lang_instruction}")        def compute_distance(self, obj: Object3D) -> float:        """计算机械臂到目标物体的欧氏距离"""        return np.linalg.norm(obj.position - self.arm_position)        def check_path_clearance(self, obj: Object3D, threshold: float = 0.3) -> bool:        """检测路径上是否有障碍物阻挡"""        direction = obj.position - self.arm_position        dist_total = np.linalg.norm(direction)        if dist_total == 0:            return True        unit_dir = direction / dist_total        for other in self.scene_objects:            if other is obj:                continue            vec_other = other.position - self.arm_position            proj_len = np.dot(vec_other, unit_dir)            if 0 < proj_len < dist_total:                perp_dist = np.linalg.norm(vec_other - proj_len * unit_dir)                if perp_dist < (other.size[0] / 2 + threshold):                    return False        return True        def generate_grasp_pose(self, obj: Object3D) -> GraspPose:        """生成抓取位姿（简化版：从机械臂正前方接近）"""        offset = 0.1  # 接近距离        dx = obj.position[0] - self.arm_position[0]        dy = obj.position[1] - self.arm_position[1]        angle = np.arctan2(dy, dx)        gx = obj.position[0] - offset * np.cos(angle)        gy = obj.position[1] - offset * np.sin(angle)        return GraspPose(x=gx, y=gy, angle=angle)        def execute_pipeline(self, instruction: str) -> dict:        """执行完整的3D-VLA空间推理流程"""        # 1. 定位目标        target_obj, language = self.find_object(instruction)                # 2. 计算距离        distance = self.compute_distance(target_obj)                # 3. 检查路径        is_clear = self.check_path_clearance(target_obj)                # 4. 生成抓取位姿        grasp = self.generate_grasp_pose(target_obj)                return {            "语言指令": language,            "目标物体": target_obj.name,            "三维位置": target_obj.position.tolist(),            "距离(m)": round(distance, 3),            "路径畅通": is_clear,            "抓取位姿": f"(x={grasp.x:.2f}, y={grasp.y:.2f}, θ={grasp.angle:.2f})"        } def main():    # 初始化3D-VLA推理器    reasoner = VLA3DSpatialReasoner()        # 构建厨房场景（模拟三维点云数据）    reasoner.add_object(Object3D("soy_sauce", np.array([0.8, 0.5, 0.0]), np.array([0.06, 0.06, 0.2])))    reasoner.add_object(Object3D("salt_shaker", np.array([0.3, 0.2, 0.0]), np.array([0.05, 0.05, 0.1])))    reasoner.add_object(Object3D("oil_bottle", np.array([0.6, 0.9, 0.0]), np.array([0.07, 0.07, 0.25])))        # 设置机械臂起始位置    reasoner.arm_position = np.array([0.0, 0.0, 0.0])        # 执行多个语言指令的推理测试    instructions = ["酱油瓶", "盐罐", "油壶"]    for instr in instructions:        try:            result = reasoner.execute_pipeline(instr)            print(f"\n指令: {result['语言指令']}")            print(f"目标: {result['目标物体']} | 位置: {result['三维位置']}")            print(f"距离: {result['距离(m)']}m | 路径畅通: {'是' if result['路径畅通'] else '否'}")            print(f"抓取位姿: {result['抓取位姿']}")        except Exception as e:            print(f"指令无法执行: {e}") if __name__ == "__main__":    main()