2026年具身智能技术深度解析：从仿真平台到物理世界的AI革命

"""
具身智能感知层 - RGB-D深度相机数据采集
依赖: pip install pyrealsense2 opencv-python numpy
"""
import cv2
import numpy as np
import pyrealsense2 as rs

class DepthCamera:
    """Intel RealSense D455 RGB-D相机封装"""
    
    def __init__(self, width=640, height=480, fps=30):
        self.width = width
        self.height = height
        self.fps = fps
        self.pipeline = rs.pipeline()
        self.config = rs.config()
        
        # 配置RGB和深度流
        self.config.enable_stream(
            rs.stream.color, 
            self.width, 
            self.height, 
            rs.format.bgr8, 
            self.fps
        )
        self.config.enable_stream(
            rs.stream.depth, 
            self.width, 
            self.height, 
            rs.format.z16, 
            self.fps
        )
        
    def start(self):
        """启动相机"""
        self.profile = self.pipeline.start(self.config)
        print(f"相机已启动: {self.width}x{self.height} @ {self.fps}fps")
        
        # 获取深度标定信息用于后续点云转换
        depth_sensor = self.profile.get_device().first_depth_sensor()
        self.depth_scale = depth_sensor.get_depth_scale()
        print(f"深度标定: {self.depth_scale:.6f} 米/单位")
        
    def capture(self):
        """捕获一帧RGB-D数据"""
        frames = self.pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        depth_frame = frames.get_depth_frame()
        
        if not color_frame or not depth_frame:
            return None, None
            
        # 转换为numpy数组
        color_image = np.asanyarray(color_frame.get_data())
        depth_image = np.asanyarray(depth_frame.get_data())
        
        return color_image, depth_image
    
    def depth_to_pointcloud(self, depth_image, fx=385.0, fy=385.0, cx=319.5, cy=239.5):
        """
        将深度图转换为3D点云
        
        Args:
            depth_image: 深度图像素值
            fx, fy: 相机内参(焦距)
            cx, cy: 相机内参(主点)
            
        Returns:
            points: Nx3 numpy数组，每行[x, y, z]
        """
        h, w = depth_image.shape
        u, v = np.meshgrid(np.arange(w), np.arange(h))
        
        # 过滤无效深度(0表示无数据)
        valid = depth_image > 0
        
        # 转换为米
        z = depth_image[valid].astype(float) * self.depth_scale
        x = ((u[valid] - cx) * z / fx).astype(np.float32)
        y = ((v[valid] - cy) * z / fy).astype(np.float32)
        
        return np.column_stack([x, y, z])
    
    def segment_graspable_objects(self, depth_image, min_area=1000):
        """
        分割可抓取物体区域
        基于深度不连续性检测物体边缘
        
        Returns:
            masks: 分割掩码列表
            bboxes: 边界框列表 [x, y, w, h]
        """
        # Sobel边缘检测
        sobelx = cv2.Sobel(depth_image, cv2.CV_16S, 1, 0)
        sobely = cv2.Sobel(depth_image, cv2.CV_16S, 0, 1)
        edges = np.sqrt(sobelx**2 + sobely**2).astype(np.uint8)
        
        # 二值化
        _, binary = cv2.threshold(edges, 30, 255, cv2.THRESH_BINARY)
        
        # 形态学操作去噪
        kernel = np.ones((5, 5), np.uint8)
        binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
        binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
        
        # 连通域分析
        num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(binary)
        
        masks = []
        bboxes = []
        
        for i in range(1, num_labels):  # 跳过背景
            area = stats[i, cv2.CC_STAT_AREA]
            if area >= min_area:
                mask = (labels == i).astype(np.uint8) * 255
                masks.append(mask)
                
                x = stats[i, cv2.CC_STAT_LEFT]
                y = stats[i, cv2.CC_STAT_TOP]
                w = stats[i, cv2.CC_STAT_WIDTH]
                h = stats[i, cv2.CC_STAT_HEIGHT]
                bboxes.append([x, y, w, h])
                
        return masks, bboxes

    def stop(self):
        """停止相机"""
        self.pipeline.stop()
        print("相机已停止")

# 使用示例
if __name__ == "__main__":
    camera = DepthCamera()
    camera.start()
    
    try:
        for _ in range(100):  # 采集100帧
            color, depth = camera.capture()
            if color is not None:
                # 提取可抓取物体
                masks, bboxes = camera.segment_graspable_objects(depth)
                
                # 生成点云
                points = camera.depth_to_pointcloud(depth)
                print(f"点云点数: {len(points)}")
                
                # 可视化
                cv2.imshow("RGB", color)
                cv2.imshow("Depth", (depth / 10).astype(np.uint8))  # 缩放显示
                
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
    finally:
        camera.stop()
        cv2.destroyAllWindows()

"""
具身智能感知层 - 触觉传感器数据采集
支持：Robotiq gripper, ATI force/torque sensor
"""

from dataclasses import dataclass
from typing import List, Optional
import struct
import serial

@dataclass
class TactileReading:
    """单点触觉读数"""
    pressure: float      # 压力值 (N)
    temperature: float   # 温度 (℃)
    timestamp: float     # 时间戳 (s)

@dataclass
class ForceTorqueReading:
    """六维力/力矩读数"""
    fx: float  # X方向力 (N)
    fy: float  # Y方向力 (N)
    fz: float  # Z方向力 (N)
    tx: float  # X方向力矩 (Nm)
    ty: float  # Y方向力矩 (Nm)
    tz: float  # Z方向力矩 (Nm)
    timestamp: float

class RobotiqGripper:
    """Robotiq 2F gripper 触觉夹爪"""
    
    # Modbus寄存器地址
    REG_STATUS = 0x07D0
    REG_POSITION = 0x07D1
    REG_FORCE = 0x07D2
    REG_SPEED = 0x07D3
    
    def __init__(self, port='/dev/ttyUSB0', baudrate=115200):
        self.serial = serial.Serial(port, baudrate, timeout=1)
        
    def activate(self):
        """激活夹爪"""
        # 激活命令
        activation = bytes([0x09, 0x10, 0x03, 0xE8, 0x00, 0x03, 
                           0x06, 0x01, 0x00, 0x00, 0x00, 0xFF, 0x00])
        self.serial.write(activation)
        
    def set_position(self, position: int, speed: int = 255, force: int = 255):
        """
        设置夹爪位置
        
        Args:
            position: 0-255, 0=全开, 255=全闭
            speed: 0-255, 速度
            force: 0-255, 力阈值
        """
        cmd = bytes([
            0x09, 0x10, 0x07D1, 0x00, 0x02, 0x04,
            position, force, speed, speed, 0x00, 0x00
        ])
        self.serial.write(cmd)
        
    def get_tactile_data(self) -> List[TactileReading]:
        """获取触觉传感数据"""
        readings = []
        for i in range(11):  # 11个传感单元
            # 模拟数据采集
            readings.append(TactileReading(
                pressure=np.random.uniform(0, 10),
                temperature=25.0 + np.random.uniform(0, 2),
                timestamp=time.time()
            ))
        return readings

class ATIForceTorqueSensor:
    """ATI Nano25 六维力传感器"""
    
    def __init__(self, calibration_matrix: np.ndarray):
        """
        Args:
            calibration_matrix: 6x6 校准矩阵
        """
        self.calib_matrix = calibration_matrix
        self.bias = np.zeros(6)
        
    def tare(self):
        """零点校准"""
        print("执行零点校准，请确保传感器无负载...")
        # 采集10秒平均值作为零点
        samples = []
        for _ in range(100):
            raw = self._read_raw()
            samples.append(raw)
            time.sleep(0.1)
        self.bias = np.mean(samples, axis=0)
        print(f"零点已校准: {self.bias}")
        
    def _read_raw(self) -> np.ndarray:
        """读取原始数据（需要硬件驱动）"""
        # 模拟数据
        return np.random.randn(6)
        
    def read(self) -> ForceTorqueReading:
        """读取校准后的力/力矩数据"""
        raw = self._read_raw()
        calibrated = self.calib_matrix @ (raw - self.bias)
        
        return ForceTorqueReading(
            fx=calibrated[0],
            fy=calibrated[1],
            fz=calibrated[2],
            tx=calibrated[3],
            ty=calibrated[4],
            tz=calibrated[5],
            timestamp=time.time()
        )

"""
具身智能认知层 - VLA视觉语言动作模型推理
基于开源VLA模型实现: OpenVLA, π0, RDT
"""
import torch
import torch.nn as nn
from transformers import AutoModel, AutoProcessor
from dataclasses import dataclass
from typing import Dict, List, Optional, Tuple

@dataclass
class VLAOutput:
    """VLA模型输出"""
    action: np.ndarray          # 动作向量 [action_dim]
    confidence: float           # 动作置信度
    language_goal: str          # 语言目标描述
    reasoning: str              # 推理过程

class VLADecoder(nn.Module):
    """VLA解码器 - 将隐表示解码为动作"""
    
    def __init__(self, hidden_dim: int, action_dim: int, lang_dim: int):
        super().__init__()
        self.action_dim = action_dim
        
        # 动作预测头
        self.action_head = nn.Sequential(
            nn.Linear(hidden_dim + lang_dim, 1024),
            nn.ReLU(),
            nn.Dropout(0.1),
            nn.Linear(1024, 512),
            nn.ReLU(),
            nn.Linear(512, action_dim * 2)  # 均值 + 对数方差
        )
        
        # 置信度预测头
        self.confidence_head = nn.Sequential(
            nn.Linear(hidden_dim + lang_dim, 256),
            nn.ReLU(),
            nn.Linear(256, 1),
            nn.Sigmoid()
        )
        
    def forward(
        self, 
        vision_features: torch.Tensor,
        lang_features: torch.Tensor
    ) -> Tuple[torch.Tensor, torch.Tensor]:
        """
        Args:
            vision_features: [B, hidden_dim] 视觉特征
            lang_features: [B, lang_dim] 语言特征
            
        Returns:
            action_mean: [B, action_dim] 动作均值
            confidence: [B, 1] 置信度
        """
        # 融合视觉和语言特征
        fused = torch.cat([vision_features, lang_features], dim=-1)
        
        # 预测动作分布参数
        action_params = self.action_head(fused)
        action_mean, action_logvar = torch.chunk(action_params, 2, dim=-1)
        
        # 预测置信度
        confidence = self.confidence_head(fused)
        
        return action_mean, confidence

class VLAModel:
    """VLA视觉语言动作模型封装"""
    
    def __init__(
        self,
        model_path: str = "openvla/openvla-7b",
        action_dim: int = 7,
        action_scale: List[float] = None,
        device: str = "cuda" if torch.cuda.is_available() else "cpu"
    ):
        self.device = device
        self.action_dim = action_dim
        self.action_scale = action_scale or [0.05, 0.05, 0.05, 0.1, 0.1, 0.1, 1.0]
        
        print(f"加载VLA模型: {model_path}")
        self.model = AutoModel.from_pretrained(
            model_path,
            torch_dtype=torch.float16,
            device_map="auto"
        )
        self.processor = AutoProcessor.from_pretrained(model_path)
        self.model.eval()
        
    @torch.no_grad()
    def predict_action(
        self,
        image: np.ndarray,
        language_goal: str,
        temperature: float = 1.0
    ) -> VLAOutput:
        """
        预测动作
        
        Args:
            image: RGB图像 [H, W, 3]
            language_goal: 语言指令，如"pick up the red cup"
            temperature: 采样温度
            
        Returns:
            VLAOutput包含预测动作
        """
        # 图像预处理
        inputs = self.processor(
            text=language_goal,
            images=image,
            return_tensors="pt"
        ).to(self.device)
        
        # 编码
        vision_features = self.model.get_vision_features(inputs["pixel_values"])
        lang_features = self.model.get_text_features(inputs["input_ids"])
        
        # 解码动作
        action_mean, confidence = self.model.decode(
            vision_features, 
            lang_features,
            temperature
        )
        
        # 缩放动作到物理范围
        action_scaled = action_mean.cpu().numpy() * np.array(self.action_scale)
        
        # 过滤异常动作
        action_clipped = np.clip(action_scaled, -1, 1)
        
        return VLAOutput(
            action=action_clipped,
            confidence=confidence.cpu().item(),
            language_goal=language_goal,
            reasoning="VLA模型基于视觉和语言输入生成动作"
        )
    
    def predict_with_ensemble(
        self,
        image: np.ndarray,
        language_goal: str,
        n_models: int = 3
    ) -> VLAOutput:
        """
        集成预测 - 使用多个VLA模型提高鲁棒性
        
        Args:
            image: RGB图像
            language_goal: 语言指令
            n_models: 集成模型数量
        """
        actions = []
        confidences = []
        
        for _ in range(n_models):
            output = self.predict_action(image, language_goal)
            actions.append(output.action)
            confidences.append(output.confidence)
            
        # 加权平均
        weights = np.array(confidences) / sum(confidences)
        ensemble_action = np.average(actions, axis=0, weights=weights)
        
        return VLAOutput(
            action=ensemble_action,
            confidence=np.mean(confidences),
            language_goal=language_goal,
            reasoning=f"集成{n_models}个模型的加权平均预测"
        )

# 使用示例
def demo_vla_inference():
    """VLA模型推理演示"""
    import cv2
    
    # 初始化模型
    vla = VLAModel(
        model_path="openvla/openvla-7b",
        action_dim=7  # 7DoF动作: [x, y, z, roll, pitch, yaw, gripper]
    )
    
    # 模拟相机输入
    image = cv2.imread("robot_camera.jpg")
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    
    # 执行抓取任务
    language_goal = "pick up the red cup on the table"
    output = vla.predict_with_ensemble(image, language_goal, n_models=5)
    
    print(f"预测动作: {output.action}")
    print(f"置信度: {output.confidence:.3f}")
    print(f"推理: {output.reasoning}")
    
    return output

"""
具身智能认知层 - 层次化任务规划器
基于LLM + PDDL规划 + 技能库
"""
import re
from dataclasses import dataclass, field
from typing import List, Dict, Optional, Set
from enum import Enum

class TaskStatus(Enum):
    PENDING = "pending"
    RUNNING = "running"
    COMPLETED = "completed"
    FAILED = "failed"

@dataclass
class Task:
    """任务节点"""
    name: str
    description: str
    subtasks: List['Task'] = field(default_factory=list)
    preconditions: Set[str] = field(default_factory=set)
    effects: Set[str] = field(default_factory=set)
    skill_name: Optional[str] = None
    parameters: Dict = field(default_factory=dict)
    status: TaskStatus = TaskStatus.PENDING
    execution_result: Optional[Dict] = None
    
    def is_executable(self, world_state: Set[str]) -> bool:
        """检查前置条件是否满足"""
        return self.preconditions <= world_state

@dataclass
class Plan:
    """执行计划"""
    tasks: List[Task]
    current_index: int = 0
    
    def get_current_task(self) -> Optional[Task]:
        if self.current_index < len(self.tasks):
            return self.tasks[self.current_index]
        return None
    
    def advance(self):
        self.current_index += 1

class Skill:
    """技能封装"""
    
    def __init__(self, name: str, action_space: List[str]):
        self.name = name
        self.action_space = action_space
        
    def generate_actions(
        self, 
        task: Task,
        world_state: Set[str]
    ) -> List[Dict]:
        """
        生成底层动作序列
        
        Returns:
            actions: 动作列表，每个动作包含{type, params, duration}
        """
        raise NotImplementedError

class GraspSkill(Skill):
    """抓取技能"""
    
    def __init__(self):
        super().__init__("grasp", ["move_to", "approach", "grasp", "lift"])
        
    def generate_actions(
        self,
        task: Task,
        world_state: Set[str]
    ) -> List[Dict]:
        obj = task.parameters.get("object", "unknown")
        
        return [
            {"type": "move_to", "position": "pre_grasp", "duration": 2.0},
            {"type": "approach", "object": obj, "duration": 1.0},
            {"type": "grasp", "object": obj, "force": 10.0, "duration": 0.5},
            {"type": "lift", "object": obj, "height": 0.1, "duration": 1.0}
        ]

class PlaceSkill(Skill):
    """放置技能"""
    
    def __init__(self):
        super().__init__("place", ["move_to", "descend", "release", "retract"])
        
    def generate_actions(
        self,
        task: Task,
        world_state: Set[str]
    ) -> List[Dict]:
        location = task.parameters.get("location", "default")
        
        return [
            {"type": "move_to", "position": "above_target", "duration": 2.0},
            {"type": "descend", "location": location, "duration": 1.5},
            {"type": "release", "force": 0.0, "duration": 0.5},
            {"type": "retract", "height": 0.2, "duration": 1.0}
        ]

class HierarchicalPlanner:
    """层次化任务规划器"""
    
    def __init__(self):
        self.skills: Dict[str, Skill] = {
            "grasp": GraspSkill(),
            "place": PlaceSkill(),
        }
        
        # LLM规划器（简化实现）
        self.prompt_template = """
你是一个机器人任务规划器。

当前世界状态: {world_state}
用户目标: {goal}

请将目标分解为可执行的任务序列，输出JSON格式：
{{
  "tasks": [
    {{
      "name": "任务名称",
      "skill": "使用的技能名",
      "parameters": {{"参数": "值"}},
      "preconditions": ["前置条件"],
      "effects": ["效果"]
    }}
  ]
}}
"""
        
    def parse_llm_response(self, response: str) -> List[Task]:
        """解析LLM响应，提取任务列表"""
        # 简化实现：正则匹配JSON
        json_match = re.search(r'\{.*\}', response, re.DOTALL)
        if json_match:
            import json
            data = json.loads(json_match.group())
            tasks = []
            for t in data.get("tasks", []):
                task = Task(
                    name=t["name"],
                    description=t["name"],
                    skill_name=t.get("skill"),
                    parameters=t.get("parameters", {}),
                    preconditions=set(t.get("preconditions", [])),
                    effects=set(t.get("effects", []))
                )
                tasks.append(task)
            return tasks
        return []
        
    def plan(
        self,
        goal: str,
        world_state: Set[str],
        use_llm: bool = True
    ) -> Plan:
        """
        生成任务计划
        
        Args:
            goal: 用户目标
            world_state: 当前世界状态
            use_llm: 是否使用LLM进行高层规划
        """
        if use_llm:
            # 调用LLM进行高层规划
            prompt = self.prompt_template.format(
                world_state=list(world_state),
                goal=goal
            )
            # 实际使用需要调用LLM API
            llm_response = self._call_llm(prompt)
            tasks = self.parse_llm_response(llm_response)
        else:
            # 基于规则的规划
            tasks = self._rule_based_planning(goal, world_state)
            
        return Plan(tasks=tasks)
    
    def _call_llm(self, prompt: str) -> str:
        """调用LLM（需要集成实际API）"""
        # 模拟返回
        return '''
        {
          "tasks": [
            {"name": "move_to_table", "skill": "move", "parameters": {"target": "table"}, "preconditions": [], "effects": ["at_table"]},
            {"name": "grasp_cup", "skill": "grasp", "parameters": {"object": "cup"}, "preconditions": ["at_table"], "effects": ["holding_cup"]},
            {"name": "move_to_sink", "skill": "move", "parameters": {"target": "sink"}, "preconditions": ["holding_cup"], "effects": ["at_sink"]},
            {"name": "place_cup", "skill": "place", "parameters": {"location": "sink"}, "preconditions": ["at_sink"], "effects": ["cup_at_sink"]}
          ]
        }
        '''
    
    def _rule_based_planning(
        self,
        goal: str,
        world_state: Set[str]
    ) -> List[Task]:
        """基于规则的简单规划"""
        tasks = []
        
        if "grasp" in goal.lower():
            obj_match = re.search(r'the (\w+)', goal)
            obj = obj_match.group(1) if obj_match else "object"
            tasks.append(Task(
                name=f"grasp_{obj}",
                description=f"抓取{obj}",
                skill_name="grasp",
                parameters={"object": obj},
                preconditions=set(),
                effects={f"holding_{obj}"}
            ))
            
        if "place" in goal.lower() or "put" in goal.lower():
            loc_match = re.search(r'on the (\w+)|in the (\w+)', goal)
            loc = loc_match.group(1) or loc_match.group(2) or "table"
            tasks.append(Task(
                name=f"place_at_{loc}",
                description=f"放置到{loc}",
                skill_name="place",
                parameters={"location": loc},
                preconditions=set(),
                effects={f"at_{loc}"}
            ))
            
        return tasks
    
    def execute_plan(
        self,
        plan: Plan,
        world_state: Set[str],
        skill_executor: callable
    ) -> bool:
        """
        执行计划
        
        Args:
            plan: 执行计划
            skill_executor: 技能执行器回调
            
        Returns:
            是否全部成功
        """
        while True:
            task = plan.get_current_task()
            if task is None:
                break
                
            # 检查前置条件
            if not task.is_executable(world_state):
                print(f"任务{task.name}前置条件不满足: {task.preconditions - world_state}")
                return False
            
            task.status = TaskStatus.RUNNING
            print(f"执行任务: {task.name}")
            
            # 获取技能
            skill = self.skills.get(task.skill_name)
            if skill is None:
                print(f"未找到技能: {task.skill_name}")
                task.status = TaskStatus.FAILED
                return False
                
            # 生成动作
            actions = skill.generate_actions(task, world_state)
            
            # 执行动作
            for action in actions:
                result = skill_executor(action)
                if not result.get("success", False):
                    task.status = TaskStatus.FAILED
                    return False
                    
            # 更新状态
            world_state |= task.effects
            task.status = TaskStatus.COMPLETED
            task.execution_result = {"actions_executed": len(actions)}
            
            plan.advance()
            
        return True

"""
具身智能仿真层 - MT Lambda-Sim物理仿真平台集成
依赖: mujoco, numpy, trimesh
"""
import mujoco
import mujoco.viewer
import numpy as np
from typing import Dict, List, Tuple, Optional
from dataclasses import dataclass
import json

@dataclass
class SimConfig:
    """仿真配置"""
    timestep: float = 0.002  # 仿真步长(s)
    n_substeps: int = 10     # 物理子步数
    solver: str = "Newton"   # 求解器
    integrator: str = "RK4"  # 积分器
    
class RobotModel:
    """仿真机器人模型"""
    
    def __init__(self, xml_path: str):
        self.model = mujoco.MjModel.from_xml_path(xml_path)
        self.data = mujoco.MjData(self.model)
        
        # 获取关节和执行器ID
        self.joint_ids = [i for i in range(self.model.njnt) 
                         if self.model.jnt_type[i] != mujoco.mjtJoint.mjJNT_FREE]
        self.actuator_ids = list(range(self.model.nu))
        
    def set_qpos(self, qpos: np.ndarray):
        """设置关节位置"""
        self.data.qpos[:len(qpos)] = qpos
        
    def set_qvel(self, qvel: np.ndarray):
        """设置关节速度"""
        self.data.qvel[:len(qvel)] = qvel
        
    def set_ctrl(self, ctrl: np.ndarray):
        """设置控制指令"""
        self.data.ctrl[:len(ctrl)] = ctrl
        
    def get_qpos(self) -> np.ndarray:
        """获取关节位置"""
        return self.data.qpos[self.joint_ids].copy()
    
    def get_qvel(self) -> np.ndarray:
        """获取关节速度"""
        return self.data.qvel[self.joint_ids].copy()
    
    def get_sensor_data(self) -> Dict[str, np.ndarray]:
        """获取传感器数据"""
        return {
            "joint_position": self.get_qpos(),
            "joint_velocity": self.get_qvel(),
            "torque": self.data.actuator_force.copy(),
            "base_position": self.data.qpos[:3].copy(),
            "base_orientation": self.data.qpos[3:7].copy(),
        }

class MTLambdaSim:
    """MT Lambda-Sim仿真平台封装"""
    
    def __init__(self, config: SimConfig = None):
        self.config = config or SimConfig()
        
        # 初始化物理引擎
        self.physics = None
        self.robot = None
        
        # 渲染器
        self.renderer = None
        
    def load_scene(self, xml_content: str):
        """加载仿真场景"""
        self.physics = mujoco.MjModel.from_xml_string(xml_content)
        self.data = mujoco.MjData(self.physics)
        
        # 设置仿真参数
        self.physics.opt.timestep = self.config.timestep
        self.physics.opt.solver = (
            mujoco.mjtSolver.mjSOL_NEWTON 
            if self.config.solver == "Newton" 
            else mujoco.mjtSolver.mjSOL_PGS
        )
        self.physics.opt.integrator = (
            mujoco.mjtIntegrator.mjINT_RK4
            if self.config.integrator == "RK4"
            else mujoco.mjtIntegrator.mjINT_EULER
        )
        
    def load_robot(self, xml_path: str) -> RobotModel:
        """加载机器人模型"""
        self.robot = RobotModel(xml_path)
        return self.robot
        
    def step(self):
        """执行一步仿真"""
        mujoco.mj_step(self.physics, self.data, nstep=self.config.n_substeps)
        
    def render(self, mode: str = "rgb_array") -> Optional[np.ndarray]:
        """渲染画面"""
        if mode == "rgb_array":
            renderer = mujoco.Renderer(self.physics, height=480, width=640)
            renderer.update_scene(self.data)
            return renderer.render()
        elif mode == "heightmap":
            return self._render_heightmap()
        return None
    
    def _render_heightmap(self) -> np.ndarray:
        """渲染高度图（用于视觉导航）"""
        hmap_res = 64
        hmap_extent = 2.0  # 米
        
        # 创建渲染器
        renderer = mujoco.Renderer(
            self.physics, 
            height=hmap_res, 
            width=hmap_res
        )
        
        # 俯视角度
        camera = mujoco.MjvCamera()
        camera.type = mujoco.mjtCamera.mjCAMERA_FIXED
        camera.trackbodyid = -1
        camera.distance = hmap_extent * 2
        camera.elevation = -90  # 俯视
        camera.azimuth = 0
        
        renderer.update_scene(self.data, camera)
        return renderer.render()
    
    def add_body(self, name: str, pos: np.ndarray, size: Tuple[float, ...]):
        """添加物体到场景"""
        body_id = mujoco.mj_name2id(
            self.physics, mujoco.mjtObj.mjOBJ_BODY, name
        )
        if body_id == -1:
            # 创建新物体（简化实现）
            pass
        else:
            # 更新现有物体位置
            self.data.body_pos[body_id] = pos
            
    def apply_force(
        self, 
        body_name: str, 
        force: np.ndarray,
        point: np.ndarray = None
    ):
        """施加外力"""
        body_id = mujoco.mj_name2id(
            self.physics, mujoco.mjtObj.mjOBJ_BODY, body_name
        )
        if body_id != -1:
            if point is None:
                point = self.data.body_pos[body_id]
            idx = body_id * 3
            self.data.xfrc_applied[idx:idx+3] = force
    
    def get_contact_info(self) -> List[Dict]:
        """获取接触信息"""
        contacts = []
        for i in range(self.data.ncon):
            contact = self.data.contact[i]
            
            # 获取接触几何体
            geom1 = mujoco.mj_id2name(
                self.physics, mujoco.mjtObj.mjOBJ_GEOM, contact.geom1
            )
            geom2 = mujoco.mj_id2name(
                self.physics, mujoco.mjtObj.mjOBJ_GEOM, contact.geom2
            )
            
            contacts.append({
                "body1": geom1,
                "body2": geom2,
                "force": contact.friction,
                "position": contact.pos,
                "normal": contact.frame[:3]
            })
        return contacts

class DataCollector:
    """仿真数据采集器"""
    
    def __init__(self, sim: MTLambdaSim):
        self.sim = sim
        self.observations = []
        self.actions = []
        self.rewards = []
        
    def record_step(
        self,
        observation: Dict,
        action: np.ndarray,
        reward: float
    ):
        """记录单步数据"""
        self.observations.append(observation)
        self.actions.append(action)
        self.rewards.append(reward)
        
    def save(self, path: str):
        """保存采集的数据"""
        data = {
            "observations": self.observations,
            "actions": self.actions,
            "rewards": self.rewards,
            "metadata": {
                "num_steps": len(self.observations),
                "sim_config": {
                    "timestep": self.sim.config.timestep,
                    "n_substeps": self.sim.config.n_substeps
                }
            }
        }
        
        with open(path, 'w') as f:
            json.dump(data, f)
            
        print(f"数据已保存到: {path}")
        print(f"共采集 {len(self.observations)} 步")

# 使用示例
def demo_simulation():
    """仿真演示"""
    
    # 创建仿真环境
    sim = MTLambdaSim(SimConfig(
        timestep=0.001,
        n_substeps=5,
        solver="Newton"
    ))
    
    # 定义场景XML
    scene_xml = """
    <mujoco model="tabletop">
        <compiler angle="degree" inertiafromgeom="true"/>
        <option timestep="0.001" integrator="RK4"/>
        
        <worldbody>
            <!-- 地面 -->
            <geom type="plane" name="ground" pos="0 0 0" size="2 2 0.1"/>
            
            <!-- 桌面 -->
            <body name="table" pos="0.5 0 0">
                <geom type="box" size="0.4 0.3 0.4" friction="1 0.005 0.005"/>
            </body>
            
            <!-- 目标物体 -->
            <body name="object" pos="0.5 0 0.45">
                <freejoint/>
                <geom type="sphere" size="0.03" rgba="1 0 0 1"/>
            </body>
            
            <!-- 机械臂 -->
            <body name="shoulder_pan" pos="0 0 0.5">
                <joint name="pan" type="hinge" axis="0 0 1" damping="0.5"/>
                <geom type="capsule" size="0.02 0.1" rgba="0.3 0.3 0.3 1"/>
                <body name="shoulder_lift" pos="0.1 0 0">
                    <joint name="lift" type="hinge" axis="0 1 0" damping="0.5"/>
                    <geom type="capsule" size="0.02 0.1"/>
                    <!-- 夹爪 -->
                    <body name="gripper" pos="0.1 0 0">
                        <joint name="grip" type="slide" axis="1 0 0" range="-0.05 0.05"/>
                        <geom type="box" size="0.02 0.01 0.02"/>
                    </body>
                </body>
            </body>
        </worldbody>
        
        <actuator>
            <motor joint="pan" ctrllimited="true" ctrlrange="-1 1" gear="100"/>
            <motor joint="lift" ctrllimited="true" ctrlrange="-1 1" gear="100"/>
            <motor joint="grip" ctrllimited="true" ctrlrange="-1 1" gear="50"/>
        </actuator>
    </mujoco>
    """
    
    sim.load_scene(scene_xml)
    
    # 创建数据采集器
    collector = DataCollector(sim)
    
    # 仿真循环
    for step in range(1000):
        # 获取观测
        obs = {
            "robot_qpos": sim.robot.get_qpos(),
            "robot_qvel": sim.robot.get_qvel(),
            "object_pos": sim.data.body("object").xpos.copy(),
            "step": step
        }
        
        # 生成控制指令（PID控制）
        ctrl = np.zeros(3)
        # ... 控制逻辑
        
        # 执行仿真
        sim.robot.set_ctrl(ctrl)
        sim.step()
        
        # 计算奖励
        reward = -np.linalg.norm(
            sim.data.body("object").xpos - sim.robot.get_qpos()[:3]
        )
        
        # 记录数据
        collector.record_step(obs, ctrl, reward)
        
        # 可视化
        if step % 100 == 0:
            img = sim.render()
            print(f"Step {step}: reward={reward:.3f}")
            
    # 保存数据
    collector.save("simulation_data.json")
    
    return sim, collector

"""
具身智能仿真层 - 域随机化(Domain Randomization)数据生成
用于生成大规模多样化训练数据
"""
import numpy as np
import trimesh
import trimesh.transformations as tf
from typing import Dict, List, Tuple, Optional
import json
import random

class DomainRandomizer:
    """域随机化器 - 增强仿真数据的泛化性"""
    
    def __init__(self):
        # 随机化参数范围
        self.params = {
            # 颜色随机化
            "object_color_range": [
                (0.1, 0.9),  # R
                (0.1, 0.9),  # G
                (0.1, 0.9),  # B
                (0.8, 1.0),  # Alpha
            ],
            
            # 形状随机化
            "shape_types": ["box", "sphere", "cylinder", "ellipsoid"],
            "shape_size_range": (0.02, 0.08),
            
            # 纹理随机化
            "texture_types": ["solid", "checker", "striped", "noise"],
            "texture_scale_range": (0.5, 2.0),
            
            # 物理属性随机化
            "mass_range": (0.05, 0.5),           # kg
            "friction_range": (0.1, 1.0),
            "restitution_range": (0.0, 0.8),     # 弹性系数
            
            # 光照随机化
            "light_intensity_range": (300, 1000),  # lux
            "light_position_range": ((-2, -2, 2), (2, 2, 5)),
            
            # 相机随机化
            "camera_distance_range": (0.3, 1.0),  # m
            "camera_angle_range": (-30, 30),      # degree
            "camera_noise_std": 0.01,
            
            # 背景随机化
            "background_types": ["solid", "texture", "complex"],
            "background_color_range": ((0.1, 0.1, 0.1), (0.9, 0.9, 0.9)),
        }
        
    def randomize_color(self) -> Tuple[float, float, float, float]:
        """随机生成RGBA颜色"""
        return tuple(
            np.random.uniform(low, high) 
            for low, high in self.params["object_color_range"]
        )
    
    def randomize_shape(self) -> Dict:
        """随机生成物体形状参数"""
        shape_type = random.choice(self.params["shape_types"])
        size = np.random.uniform(*self.params["shape_size_range"])
        
        shape_params = {"type": shape_type, "size": size}
        
        if shape_type == "box":
            shape_params["dims"] = (
                size * np.random.uniform(0.5, 1.5),
                size * np.random.uniform(0.5, 1.5),
                size * np.random.uniform(0.5, 1.5)
            )
        elif shape_type == "sphere":
            shape_params["radius"] = size
        elif shape_type == "cylinder":
            shape_params["radius"] = size * np.random.uniform(0.5, 1.0)
            shape_params["height"] = size * np.random.uniform(1.0, 2.0)
        elif shape_type == "ellipsoid":
            shape_params["radii"] = (
                size * np.random.uniform(0.5, 1.5),
                size * np.random.uniform(0.5, 1.5),
                size * np.random.uniform(0.5, 1.5)
            )
            
        return shape_params
    
    def randomize_physics(self) -> Dict:
        """随机生成物理属性"""
        return {
            "mass": np.random.uniform(*self.params["mass_range"]),
            "friction": np.random.uniform(*self.params["friction_range"]),
            "restitution": np.random.uniform(*self.params["restitution_range"]),
        }
    
    def randomize_lighting(self) -> Dict:
        """随机生成光照条件"""
        pos_low, pos_high = self.params["light_position_range"]
        position = np.random.uniform(pos_low, pos_high)
        
        return {
            "position": position,
            "intensity": np.random.uniform(*self.params["light_intensity_range"]),
            "ambient": np.random.uniform(0.1, 0.4),
            "diffuse": np.random.uniform(0.4, 0.8),
        }
    
    def randomize_camera(self, base_pose: Dict) -> Dict:
        """随机生成相机参数"""
        distance = np.random.uniform(*self.params["camera_distance_range"])
        azimuth = np.deg2rad(np.random.uniform(*self.params["camera_angle_range"]))
        elevation = np.deg2rad(np.random.uniform(-20, 60))
        
        # 计算相机位置
        x = base_pose["target"][0] + distance * np.cos(elevation) * np.sin(azimuth)
        y = base_pose["target"][1] + distance * np.cos(elevation) * np.cos(azimuth)
        z = base_pose["target"][2] + distance * np.sin(elevation)
        
        return {
            "position": np.array([x, y, z]),
            "target": np.array(base_pose["target"]),
            "noise_std": self.params["camera_noise_std"]
        }
    
    def generate_randomized_scene(self) -> Dict:
        """生成完整随机化场景"""
        scene = {
            "objects": [],
            "lighting": self.randomize_lighting(),
            "camera": self.randomize_camera({
                "target": [0.5, 0, 0.5]
            }),
            "background": self._randomize_background(),
        }
        
        # 生成3-8个随机物体
        n_objects = np.random.randint(3, 9)
        for i in range(n_objects):
            obj = {
                "id": f"object_{i}",
                "shape": self.randomize_shape(),
                "color": self.randomize_color(),
                "physics": self.randomize_physics(),
                "position": self._randomize_position(),
                "orientation": tf.random_rotation_matrix().flatten()[:9].reshape(3, 3),
            }
            scene["objects"].append(obj)
            
        return scene
    
    def _randomize_position(self) -> np.ndarray:
        """在工作空间内随机生成位置"""
        x = np.random.uniform(0.3, 0.7)
        y = np.random.uniform(-0.2, 0.2)
        z = np.random.uniform(0.05, 0.15)  # 桌面上方
        return np.array([x, y, z])
    
    def _randomize_background(self) -> Dict:
        """随机生成背景"""
        bg_type = random.choice(self.params["background_types"])
        color_low, color_high = self.params["background_color_range"]
        
        if bg_type == "solid":
            color = np.random.uniform(color_low, color_high)
        else:
            color = np.random.uniform(color_low, color_high)
            
        return {
            "type": bg_type,
            "color": color,
        }

class SyntheticDataGenerator:
    """合成数据生成器"""
    
    def __init__(self, randomizer: DomainRandomizer):
        self.randomizer = randomizer
        self.generated_scenes = []
        
    def generate_dataset(
        self,
        num_scenes: int,
        output_dir: str
    ) -> List[str]:
        """
        生成大规模合成数据集
        
        Args:
            num_scenes: 生成场景数量
            output_dir: 输出目录
            
        Returns:
            生成的文件路径列表
        """
        import os
        os.makedirs(output_dir, exist_ok=True)
        
        file_paths = []
        
        for i in range(num_scenes):
            # 生成随机场景
            scene = self.randomizer.generate_randomized_scene()
            
            # 生成渲染图像
            images = self._render_scene(scene)
            
            # 生成标注数据
            annotations = self._generate_annotations(scene)
            
            # 保存
            base_path = f"{output_dir}/scene_{i:06d}"
            
            # 保存场景JSON
            scene_path = f"{base_path}_scene.json"
            with open(scene_path, 'w') as f:
                json.dump(scene, f, indent=2)
                
            # 保存标注JSON
            anno_path = f"{base_path}_annotations.json"
            with open(anno_path, 'w') as f:
                json.dump(annotations, f, indent=2)
                
            # 保存图像（需要渲染器，这里简化处理）
            # cv2.imwrite(f"{base_path}_rgb.png", rgb_image)
            
            file_paths.extend([scene_path, anno_path])
            
            if (i + 1) % 1000 == 0:
                print(f"已生成 {i + 1}/{num_scenes} 场景")
                
        return file_paths
    
    def _render_scene(self, scene: Dict) -> Dict:
        """渲染场景（需要集成渲染器）"""
        # 返回占位符
        return {
            "rgb": np.zeros((480, 640, 3), dtype=np.uint8),
            "depth": np.zeros((480, 640), dtype=np.float32),
            "segmentation": np.zeros((480, 640), dtype=np.uint8),
        }
    
    def _generate_annotations(self, scene: Dict) -> Dict:
        """生成标注数据"""
        annotations = {
            "objects": [],
            "relationships": [],
        }
        
        for obj in scene["objects"]:
            # 2D边界框
            bbox_2d = self._compute_2d_bbox(obj)
            
            # 3D边界框
            bbox_3d = self._compute_3d_bbox(obj)
            
            annotations["objects"].append({
                "id": obj["id"],
                "category": "object",
                "bbox_2d": bbox_2d,
                "bbox_3d": bbox_3d,
                "color": obj["color"],
                "shape": obj["shape"]["type"],
            })
            
        # 添加空间关系
        annotations["relationships"] = self._compute_relationships(
            scene["objects"]
        )
        
        return annotations
    
    def _compute_2d_bbox(self, obj: Dict) -> List[int]:
        """计算2D边界框 [x_min, y_min, x_max, y_max]"""
        # 简化实现：基于物体位置和大小估算
        pos = obj["position"]
        size = obj["shape"].get("size", 0.05)
        
        # 投影到图像平面（简化）
        x_center = int(320 + pos[0] * 200)
        y_center = int(240 - pos[1] * 200)
        half_size = int(size * 300)
        
        return [
            max(0, x_center - half_size),
            max(0, y_center - half_size),
            min(640, x_center + half_size),
            min(480, y_center + half_size),
        ]
    
    def _compute_3d_bbox(self, obj: Dict) -> Dict:
        """计算3D边界框"""
        pos = obj["position"]
        shape = obj["shape"]
        
        size = shape.get("size", 0.05)
        
        return {
            "center": pos.tolist(),
            "size": [size * 2] * 3,
        }
    
    def _compute_relationships(
        self, 
        objects: List[Dict]
    ) -> List[Dict]:
        """计算物体间空间关系"""
        relationships = []
        
        for i, obj1 in enumerate(objects):
            for j, obj2 in enumerate(objects):
                if i >= j:
                    continue
                    
                dist = np.linalg.norm(obj1["position"] - obj2["position"])
                
                if dist < 0.15:  # 15cm
                    relationships.append({
                        "subject": obj1["id"],
                        "predicate": "near",
                        "object": obj2["id"],
                        "distance": float(dist)
                    })
                    
        return relationships

# 使用示例
def demo_domain_randomization():
    """域随机化演示"""
    
    # 创建随机化器
    randomizer = DomainRandomizer()
    
    # 生成单个随机场景
    scene = randomizer.generate_randomized_scene()
    
    print("随机化场景配置:")
    print(f"  物体数量: {len(scene['objects'])}")
    print(f"  光照强度: {scene['lighting']['intensity']:.1f} lux")
    print(f"  相机距离: {np.linalg.norm(scene['camera']['position']):.3f} m")
    
    # 生成合成数据集
    generator = SyntheticDataGenerator(randomizer)
    files = generator.generate_dataset(
        num_scenes=100,
        output_dir="./synthetic_data"
    )
    
    print(f"\n生成完成！共 {len(files)} 个文件")
    
    return files

/*
具身智能执行层 - 运动控制器 (Go语言实现)
支持: MPC模型预测控制 / WBC全身控制
依赖: go.mod github.com/go-gl/gl/v2.1/gl
*/
package controller

import (
	"math"
	"math/rand"
)

// Vector3 3D向量
type Vector3 struct {
	X, Y, Z float64
}

// Quaternion 四元数姿态
type Quaternion struct {
	W, X, Y, Z float64
}

// JointState 关节状态
type JointState struct {
	Position float64 // 位置 (rad 或 m)
	Velocity float64 // 速度 (rad/s 或 m/s)
	Torque   float64 // 力矩 (Nm 或 N)
}

// RobotState 机器人状态
type RobotState struct {
	BasePos      Vector3     // 基座位置
	BaseOri      Quaternion  // 基座姿态
	JointPos     []float64   // 关节位置
	JointVel     []float64   // 关节速度
	JointTorque  []float64   // 关节力矩
	FootContacts []bool      // 脚部接触
}

// JointLimit 关节限制
type JointLimit struct {
	Min    float64
	Max    float64
	DMin   float64 // 速度限制
	DDMin  float64 // 加速度限制
	TMin   float64 // 力矩限制
}

// JointController 关节级控制器
type JointController struct {
	P Gain     // 比例增益
	D Gain     // 微分增益
	FF float64 // 前馈增益
	
	jointLimit JointLimit
	targetPos  float64
	targetVel  float64
}

// Gain PID增益
type Gain struct {
	P, I, D float64
}

// NewJointController 创建关节控制器
func NewJointController(kp, kd float64) *JointController {
	return &JointController{
		P: Gain{P: kp},
		D: Gain{P: kd},
	}
}

// Compute 计算控制力矩
func (c *JointController) Compute(state JointState) float64 {
	// 位置误差
	posErr := c.targetPos - state.Position
	
	// 速度误差
	velErr := c.targetVel - state.Velocity
	
	// PD控制 + 前馈
	torque := c.P.P*posErr + c.D.P*velErr
	
	// 限幅
	torque = math.Max(c.jointLimit.TMin, math.Min(-c.jointLimit.TMin, torque))
	
	return torque
}

// SetTarget 设置控制目标
func (c *JointController) SetTarget(pos, vel float64) {
	c.targetPos = pos
	c.targetVel = vel
}

// WholeBodyController 全身控制器 (WBC)
type WholeBodyController struct {
	numJoints     int
	weightTask    []float64 // 任务权重
	qpg           *QPOptimizer // QP求解器
	
	// 任务定义
	tasks []Task
	
	// 机器人模型
	model *RobotModel
}

// Task 优化任务
type Task struct {
	Type     string          // "position", "orientation", "force"
	Jacobian [][]float64     // 雅可比矩阵
	Target   []float64       // 目标值
	Weight   float64         // 权重
}

// QPOptimizer 二次规划求解器
type QPOptimizer struct {
	hessian [][]float64 // 海森矩阵
	gradient []float64   // 梯度向量
	Aeq     [][]float64  // 等式约束
	beq     []float64    // 等式右边
	Aineq   [][]float64  // 不等式约束
	bineq   []float64   // 不等式右边
}

// NewWBC 创建全身控制器
func NewWBC(numJoints int) *WholeBodyController {
	return &WholeBodyController{
		numJoints: numJoints,
		weightTask: make([]float64, 10),
		qpg: new(QPOptimizer),
	}
}

// AddTask 添加优化任务
func (wbc *WholeBodyController) AddTask(task Task) {
	wbc.tasks = append(wbc.tasks, task)
}

// Compute 计算最优关节力矩
func (wbc *WholeBodyController) Compute(state RobotState) []float64 {
	// 构建QP问题
	wbc.buildQP(state)
	
	// 求解QP
	solution := wbc.qpg.Solve()
	
	// 提取关节力矩
	torques := make([]float64, wbc.numJoints)
	copy(torques, solution)
	
	return torques
}

// buildQP 构建二次规划问题
func (wbc *WholeBodyController) buildQP(state RobotState) {
	nVar := wbc.numJoints
	
	// 初始化海森矩阵和梯度
	wbc.qpg.hessian = make([][]float64, nVar)
	wbc.qpg.gradient = make([]float64, nVar)
	
	for i := 0; i < nVar; i++ {
		wbc.qpg.hessian[i] = make([]float64, nVar)
		for j := 0; j < nVar; j++ {
			wbc.qpg.hessian[i][j] = 0
		}
		wbc.qpg.gradient[i] = 0
	}
	
	// 添加各任务代价
	for _, task := range wbc.tasks {
		wbc.addTaskCost(task)
	}
	
	// 添加关节限幅约束
	wbc.addJointLimits(state)
}

// addTaskCost 添加任务代价
func (wbc *WholeBodyController) addTaskCost(task Task) {
	// 简化的任务代价: 0.5 * ||J*q_dot - target||^2
	// 对应海森矩阵: J^T * J, 梯度: -J^T * target
	
	for i := 0; i < wbc.numJoints; i++ {
		for j := 0; j < wbc.numJoints; j++ {
			hess := 0.0
			for k := 0; k < len(task.Target); k++ {
				hess += task.Jacobian[k][i] * task.Jacobian[k][j]
			}
			wbc.qpg.hessian[i][j] += hess * task.Weight
		}
		
		grad := 0.0
		for k := 0; k < len(task.Target); k++ {
			grad -= task.Jacobian[k][i] * task.Target[k]
		}
		wbc.qpg.gradient[i] += grad * task.Weight
	}
}

// addJointLimits 添加关节限幅约束
func (wbc *WholeBodyController) addJointLimits(state RobotState) {
	// 添加位置限幅约束
	for i := 0; i < wbc.numJoints; i++ {
		pos := state.JointPos[i]
		torqueLimit := 50.0 // Nm
		
		// 简化处理：直接限幅
		if pos > math.Pi {
			wbc.qpg.gradient[i] -= 1000 * (pos - math.Pi)
		}
		if pos < -math.Pi {
			wbc.qpg.gradient[i] -= 1000 * (pos + math.Pi)
		}
	}
	_ = torqueLimit
}

// Solve 求解QP问题（简化实现）
func (qp *QPOptimizer) Solve() []float64 {
	// 梯度下降法求解
	solution := make([]float64, len(qp.gradient))
	lr := 0.001
	maxIter := 1000
	
	for iter := 0; iter < maxIter; iter++ {
		for i := 0; i < len(qp.gradient); i++ {
			grad := 0.0
			for j := 0; j < len(qp.gradient); j++ {
				grad += qp.hessian[i][j] * solution[j]
			}
			grad += qp.gradient[i]
			solution[i] -= lr * grad
		}
	}
	
	return solution
}

// RobotModel 简化的机器人模型
type RobotModel struct {
	NumJoints    int
	JointLimits  []JointLimit
	MassMatrix   [][]float64
	GravityVec   []float64
}

// ComputeGravity 计算重力补偿力矩
func (m *RobotModel) ComputeGravity(q []float64) []float64 {
	torques := make([]float64, m.NumJoints)
	
	for i := 0; i < m.NumJoints; i++ {
		// 简化的重力项计算
		torques[i] = 9.81 * math.Sin(q[i]) * 0.5
	}
	
	return torques
}

// ComputeDynamics 计算动力学
func (m *RobotModel) ComputeDynamics(q, qd []float64) []float64 {
	n := m.NumJoints
	torques := make([]float64, n)
	
	// M(q) * q_dd + C(q, q_d) + g(q) = tau
	for i := 0; i < n; i++ {
		// 简化的惯性项
		torques[i] = m.MassMatrix[i][i] * 0.1
		// 科里奥利项（简化）
		torques[i] += 0.1 * qd[i]
		// 重力项
		torques[i] += m.GravityVec[i]
	}
	
	return torques
}

// ComputeFootJacobian 计算脚部雅可比矩阵
func (m *RobotModel) ComputeFootJacobian(footID int, q []float64) [][]float64 {
	// 返回简化雅可比矩阵
	jacobian := make([][]float64, 3)
	for i := range jacobian {
		jacobian[i] = make([]float64, m.NumJoints)
	}
	
	// 简化计算
	for j := 0; j < m.NumJoints; j++ {
		jacobian[0][j] = math.Sin(q[j])
		jacobian[1][j] = math.Cos(q[j])
		jacobian[2][j] = 1.0
	}
	
	return jacobian
}

// MPCController 模型预测控制器
type MPCController struct {
	horizon    int           // 预测时域
	dt         float64       // 时间步长
	model      *RobotModel   // 机器人模型
	
	// 权重
	weightState   float64
	weightControl float64
	weightForce   float64
	
	// 内部状态
	xPred  [][]float64  // 预测状态
	uPred  [][]float64  // 预测控制
}

// NewMPCController 创建MPC控制器
func NewMPCController(horizon int, dt float64) *MPCController {
	return &MPCController{
		horizon: horizon,
		dt: dt,
		xPred: make([][]float64, horizon+1),
		uPred: make([][]float64, horizon),
	}
}

// Predict 预测未来状态
func (mpc *MPCController) Predict(state []float64, target []float64) [][]float64 {
	// 初始化
	for i := 0; i <= mpc.horizon; i++ {
		mpc.xPred[i] = make([]float64, len(state))
	}
	mpc.xPred[0] = state
	
	// 迭代预测
	for t := 0; t < mpc.horizon; t++ {
		// 计算控制输入（简化：跟踪目标）
		mpc.uPred[t] = mpc.computeControl(mpc.xPred[t], target)
		
		// 状态更新
		mpc.xPred[t+1] = mpc.dynamics(mpc.xPred[t], mpc.uPred[t])
	}
	
	return mpc.xPred
}

// computeControl 计算控制输入
func (mpc *MPCController) computeControl(state, target []float64) []float64 {
	// 简化的反馈控制
	kp := 10.0
	kd := 5.0
	
	u := make([]float64, mpc.model.NumJoints)
	for i := range u {
		if i < len(state) && i < len(target) {
			u[i] = kp*(target[i] - state[i])
			if i < len(state)/2 {
				u[i] -= kd * state[i+len(state)/2]
			}
		}
	}
	
	return u
}

// dynamics 简化系统动力学
func (mpc *MPCController) dynamics(x, u []float64) []float64 {
	xNext := make([]float64, len(x))
	
	for i := range xNext {
		if i < len(x)/2 {
			// 位置 = 速度 * dt
			if i < len(u) {
				xNext[i] = x[i] + x[i+len(x)/2]*mpc.dt
			}
		} else {
			// 速度 = 加速度 * dt
			if i-len(x)/2 < len(u) {
				xNext[i] = x[i] + u[i-len(x)/2]*mpc.dt
			}
		}
	}
	
	return xNext
}

// GetFirstControl 获取第一个控制输入
func (mpc *MPCController) GetFirstControl() []float64 {
	if len(mpc.uPred) > 0 {
		return mpc.uPred[0]
	}
	return make([]float64, mpc.model.NumJoints)
}

// 辅助函数
func new(QPOptimizer) *QPOptimizer {
	return &QPOptimizer{
		hessian:  make([][]float64, 0),
		gradient: make([]float64, 0),
	}
}

"""
具身智能执行层 - 末端执行器(夹爪)控制
"""
import time
from typing import List, Tuple, Optional
from dataclasses import dataclass
import numpy as np

@dataclass
class GripperCommand:
    """夹爪控制指令"""
    position: float     # 目标位置 (0=全开, 1=全闭)
    speed: float        # 速度 (0-1)
    force: float        # 力阈值 (0-1)
    wait: bool = True   # 是否等待完成

@dataclass
class GripperStatus:
    """夹爪状态"""
    position: float
    current: float      # 电流 (A)
    is_moving: bool
    is_stalled: bool
    object_detected: bool

class GripperController:
    """夹爪控制器"""
    
    def __init__(self, config: dict = None):
        self.config = config or {
            "max_velocity": 100,      # mm/s
            "max_force": 150,          # N
            "min_width": 0,            # mm
            "max_width": 85,           # mm
            "grasp_deadzone": 5,       # mm
        }
        
        self.current_position = 0
        self.target_position = 0
        self.is_grasping = False
        
    def open(self, width: float = None, speed: float = 0.5) -> bool:
        """
        打开夹爪
        
        Args:
            width: 目标开口宽度(mm)，None表示全开
            speed: 速度 (0-1)
            
        Returns:
            是否成功
        """
        if width is None:
            width = self.config["max_width"]
            
        width = np.clip(width, self.config["min_width"], self.config["max_width"])
        self.target_position = width
        
        return self._send_command(
            GripperCommand(
                position=width / self.config["max_width"],
                speed=speed,
                force=0.3,
                wait=False
            )
        )
    
    def close(self, force: float = 0.8) -> bool:
        """
        关闭夹爪
        
        Args:
            force: 力阈值 (0-1)
            
        Returns:
            是否成功
        """
        self.target_position = 0
        self.is_grasping = True
        
        return self._send_command(
            GripperCommand(
                position=0,
                speed=0.3,
                force=force,
                wait=False
            )
        )
    
    def grasp(
        self,
        target_width: float,
        speed: float = 0.3,
        force_limit: float = 0.8
    ) -> bool:
        """
        执行抓取动作
        
        Args:
            target_width: 目标闭合宽度 (mm)
            speed: 闭合速度
            force_limit: 力限制
            
        Returns:
            是否成功抓取
        """
        # 1. 移动到目标宽度
        self.close(force=force_limit)
        
        # 2. 等待夹爪停止或检测到物体
        timeout = 5.0
        start_time = time.time()
        
        while time.time() - start_time < timeout:
            status = self.get_status()
            
            if status.object_detected:
                self.current_position = status.position
                print(f"检测到物体，停止位置: {status.position:.2f}mm")
                return True
                
            if not status.is_moving:
                # 夹爪停止但未检测到物体
                self.current_position = status.position
                if status.position < target_width:
                    print("抓取失败：位置异常")
                    return False
                return True
                
            time.sleep(0.01)
            
        print("抓取超时")
        return False
    
    def move_to(self, position: float, speed: float = 0.5) -> bool:
        """
        移动到指定位置
        
        Args:
            position: 目标位置 (mm)
            speed: 速度 (0-1)
            
        Returns:
            是否成功
        """
        position = np.clip(
            position, 
            self.config["min_width"], 
            self.config["max_width"]
        )
        self.target_position = position
        
        return self._send_command(
            GripperCommand(
                position=position / self.config["max_width"],
                speed=speed,
                force=0.5,
                wait=True
            )
        )
    
    def get_status(self) -> GripperStatus:
        """获取夹爪状态"""
        # 模拟状态读取
        is_moving = abs(self.target_position - self.current_position) > 0.5
        
        return GripperStatus(
            position=self.current_position,
            current=0.5 if is_moving else 0.1,
            is_moving=is_moving,
            is_stalled=False,
            object_detected=self.current_position < self.config["grasp_deadzone"]
        )
    
    def _send_command(self, cmd: GripperCommand) -> bool:
        """发送控制命令（需要硬件驱动）"""
        # 模拟命令发送
        print(f"发送命令: pos={cmd.position:.2f}, speed={cmd.speed:.2f}, force={cmd.force:.2f}")
        
        if cmd.wait:
            time.sleep(abs(cmd.position - self.current_position) * 0.1)
            self.current_position = cmd.position * self.config["max_width"]
            
        return True

class DualGripperController:
    """双夹爪控制器（用于协调操作）"""
    
    def __init__(self):
        self.left = GripperController()
        self.right = GripperController()
        self.sync_threshold = 5.0  # mm
        
    def grasp_both(
        self,
        left_width: float,
        right_width: float,
        tolerance: float = 2.0
    ) -> bool:
        """
        同时抓取两个物体
        
        Args:
            left_width: 左侧目标宽度
            right_width: 右侧目标宽度
            tolerance: 位置容差
            
        Returns:
            是否成功
        """
        # 启动两个夹爪
        self.left.close()
        self.right.close()
        
        # 等待两侧都完成
        max_wait = 5.0
        start = time.time()
        
        while time.time() - start < max_wait:
            left_status = self.left.get_status()
            right_status = self.right.get_status()
            
            if not left_status.is_moving and not right_status.is_moving:
                # 检查位置是否满足要求
                left_ok = abs(left_status.position - left_width) < tolerance
                right_ok = abs(right_status.position - right_width) < tolerance
                
                if left_ok and right_ok:
                    return True
                    
            time.sleep(0.01)
            
        return False
    
    def approach_and_grasp(
        self,
        left_pos: Tuple[float, float, float],
        right_pos: Tuple[float, float, float],
        approach_height: float = 0.05
    ) -> bool:
        """
        接近并抓取两个物体
        
        Args:
            left_pos: 左侧物体位置 (x, y, z)
            right_pos: 右侧物体位置 (x, y, z)
            approach_height: 接近时的高度偏移
        """
        # 1. 打开夹爪
        self.left.open()
        self.right.open()
        
        # 2. 移动到预抓取位置
        # ... (需要集成机械臂控制)
        
        # 3. 下探
        # ... 
        
        # 4. 执行抓取
        return self.grasp_both(
            left_width=10.0,   # 目标宽度
            right_width=10.0
        )

# 使用示例
def demo_gripper_control():
    """夹爪控制演示"""
    
    gripper = GripperController()
    
    # 1. 打开夹爪
    print("打开夹爪...")
    gripper.open()
    time.sleep(1)
    
    # 2. 移动到中间位置
    print("移动到中间位置...")
    gripper.move_to(40)  # 40mm
    time.sleep(1)
    
    # 3. 执行抓取
    print("执行抓取...")
    success = gripper.grasp(target_width=5.0, force_limit=0.6)
    
    if success:
        print("抓取成功！")
        
        # 获取状态
        status = gripper.get_status()
        print(f"夹爪位置: {status.position:.2f}mm")
        print(f"检测到物体: {status.object_detected}")
        
        # 移动到目标位置
        # ... (搬运操作)
        
        # 释放
        print("释放物体...")
        gripper.open()
    else:
        print("抓取失败")
        
    return success