unity2moveit2/SYSTEM_DESIGN.md

# Unity-MoveIt2 通用机械臂交互式仿真系统

## 系统概述

本系统是一个基于Unity前端渲染和MoveIt2后端规划的通用机械臂交互式仿真平台，支持1-9自由度的任意机械臂配置。系统提供类似RViz2的交互体验，用户可以直接拖拽机械臂末端执行器到目标位置，系统自动进行路径规划、碰撞检测和避障处理。

### 核心特性
- **通用性**：支持1-9自由度的任意机械臂配置
- **交互性**：实时拖拽操作，即时视觉反馈
- **智能性**：自动路径规划、碰撞检测、避障算法
- **可视化**：高质量3D渲染，直观的状态显示
- **扩展性**：模块化设计，易于扩展和定制

## 系统架构

### 整体架构图
```
┌─────────────────────────────────────┐
│           Unity 前端层               │
├─────────────────────────────────────┤
│  交互控制器  │  可视化引擎  │  UI管理器  │
│             │             │          │
│ • 拖拽控制   │ • 3D渲染    │ • 状态面板 │
│ • 手势识别   │ • 轨迹显示  │ • 参数设置 │
│ • 约束检查   │ • 碰撞高亮  │ • 信息提示 │
└─────────────────────────────────────┘
                    │
              ROS2 TCP Bridge
                    │
┌─────────────────────────────────────┐
│          MoveIt2 后端层              │
├─────────────────────────────────────┤
│  规划引擎   │  碰撞检测   │  执行监控   │
│            │            │           │
│ • 运动规划  │ • 实时检测  │ • 状态跟踪 │
│ • 轨迹优化  │ • 自碰撞   │ • 错误处理 │
│ • 约束处理  │ • 环境碰撞  │ • 安全监控 │
└─────────────────────────────────────┘
```

### 核心组件

#### 1. Unity前端组件
- **通用机械臂控制器**：支持任意自由度配置
- **交互式末端控制器**：6DOF拖拽操作，支持任务目标标注与环境编辑
- **实时可视化引擎**：轨迹、碰撞、状态显示
- **动态UI系统**：根据机械臂配置自适应界面

#### 2. MoveIt2后端组件
- **通用规划服务**：支持多种规划算法
- **动态碰撞检测**：实时环境感知
- **约束管理器**：处理各种运动约束
- **执行监控器**：轨迹执行和安全监控

#### 3. 通信桥梁
- **ROS2-TCP连接器**：Unity与ROS2的通信桥梁
- **消息转换器**：数据格式转换和同步
- **状态同步器**：实时状态信息同步

## 技术规格

### 支持的机械臂配置
- **自由度范围**：1-9 DOF
- **关节类型**：旋转关节、移动关节、固定关节
- **运动学类型**：串联、并联、混合结构
- **末端执行器**：夹爪、吸盘、工具等

### 性能指标
- **规划响应时间**：< 500ms（复杂场景）
- **碰撞检测频率**：> 100Hz
- **轨迹执行精度**：± 0.1mm
- **实时渲染帧率**：> 60 FPS

### 技术栈
- **前端**：Unity 2022.3 LTS (推荐 2022.3.57f1c2), C#
- **后端**：ROS2 Jazzy, Python/C++
- **通信**：ROS-TCP-Connector, WebSocket
- **规划**：MoveIt2, OMPL, FCL
- **容器化**：Docker, Docker Compose

## 功能模块详述

### 1. 通用机械臂适配器

#### 动态配置加载
```csharp
public class UniversalRobotAdapter : MonoBehaviour
{
    [Header("机械臂配置")]
    public int degreesOfFreedom = 6;
    public JointType[] jointTypes;
    public float[] jointLimitsMin;
    public float[] jointLimitsMax;
    public float[] jointVelocityLimits;
    
    [Header("运动学参数")]
    public Transform baseLink;
    public Transform endEffector;
    public string urdfPath;
    
    // 动态初始化机械臂
    public void InitializeRobot(RobotConfiguration config)
    {
        degreesOfFreedom = config.dof;
        LoadURDF(config.urdfPath);
        SetupJointControllers();
        ConfigureConstraints();
    }
}
```

#### 关节控制器管理
```csharp
public class JointControllerManager : MonoBehaviour
{
    private ArticulationBody[] joints;
    private JointController[] controllers;
    
    public void SetupControllers(int dof)
    {
        joints = new ArticulationBody[dof];
        controllers = new JointController[dof];
        
        for (int i = 0; i < dof; i++)
        {
            controllers[i] = new JointController(joints[i]);
        }
    }
    
    public void UpdateJointPositions(float[] positions)
    {
        for (int i = 0; i < positions.Length; i++)
        {
            controllers[i].SetTargetPosition(positions[i]);
        }
    }
}
```

### 2. 交互式末端控制系统

#### 6DOF拖拽控制器
```csharp
public class EndEffectorDragController : MonoBehaviour
{
    [Header("拖拽设置")]
    public bool enablePositionDrag = true;
    public bool enableRotationDrag = true;
    public float dragSensitivity = 1.0f;
    public float rotationSensitivity = 1.0f;
    
    [Header("约束设置")]
    public bool respectJointLimits = true;
    public bool avoidCollisions = true;
    public float workspaceRadius = 2.0f;
    
    private Vector3 targetPosition;
    private Quaternion targetRotation;
    private bool isDragging = false;
    
    void Update()
    {
        HandleMouseInput();
        if (isDragging)
        {
            UpdateTargetPose();
            RequestIKSolution();
        }
    }
    
    private void RequestIKSolution()
    {
        var request = new IKRequest
        {
            targetPose = new Pose(targetPosition, targetRotation),
            currentJoints = GetCurrentJointAngles(),
            respectLimits = respectJointLimits,
            avoidCollisions = avoidCollisions
        };
        
        rosConnector.SendIKRequest(request);
    }
}
```

#### 实时IK求解器
```csharp
public class RealTimeIKSolver : MonoBehaviour
{
    private ROSConnection rosConnection;
    private string ikServiceName = "/compute_ik";
    
    public void SolveIK(Pose targetPose, Action<float[]> onSolutionFound)
    {
        var request = new GetPositionIKRequest
        {
            ik_request = new PositionIKRequest
            {
                group_name = "manipulator",
                pose_stamped = new PoseStamped
                {
                    pose = ConvertUnityPoseToROS(targetPose)
                },
                avoid_collisions = true,
                timeout = new Duration { sec = 0, nanosec = 100000000 } // 0.1s
            }
        };
        
        rosConnection.SendServiceMessage<GetPositionIKResponse>(
            ikServiceName, request, OnIKResponse);
    }
    
    private void OnIKResponse(GetPositionIKResponse response)
    {
        if (response.error_code.val == MoveItErrorCodes.SUCCESS)
        {
            var jointAngles = ExtractJointAngles(response.solution);
            ApplyJointAngles(jointAngles);
        }
    }
}
```

### 3. 智能路径规划系统

#### 通用规划服务
```python
class UniversalPlanningService(Node):
    def __init__(self):
        super().__init__('universal_planning_service')
        
        # 服务定义
        self.plan_service = self.create_service(
            PlanToTarget, '/plan_to_target', self.plan_to_target_callback)
        self.ik_service = self.create_service(
            ComputeIK, '/compute_ik', self.compute_ik_callback)
        self.collision_service = self.create_service(
            CheckCollision, '/check_collision', self.check_collision_callback)
        
        # MoveIt接口
        self.move_group = None
        self.planning_scene = None
        self.robot_model = None
        
    def initialize_robot(self, robot_description, group_name, dof):
        """动态初始化机械臂配置"""
        self.move_group = MoveGroupInterface(group_name)
        self.planning_scene = PlanningSceneInterface()
        self.robot_model = RobotModel(robot_description)
        self.dof = dof
        
        # 配置规划参数
        self.move_group.set_planning_time(5.0)
        self.move_group.set_num_planning_attempts(10)
        self.move_group.set_max_velocity_scaling_factor(0.8)
        self.move_group.set_max_acceleration_scaling_factor(0.8)
    
    def plan_to_target_callback(self, request, response):
        """规划到目标位姿的路径"""
        try:
            # 设置目标位姿
            self.move_group.set_pose_target(request.target_pose)
            
            # 执行规划
            plan = self.move_group.plan()
            
            if plan[0]:  # 规划成功
                response.trajectory = plan[1]
                response.success = True
                response.planning_time = plan[2].planning_time
            else:
                response.success = False
                response.error_message = "规划失败"
                
        except Exception as e:
            response.success = False
            response.error_message = str(e)
            
        return response
    
    def compute_ik_callback(self, request, response):
        """计算逆运动学解"""
        try:
            # 使用KDL或MoveIt的IK求解器
            joint_angles = self.solve_ik(
                request.target_pose, 
                request.seed_state,
                request.timeout
            )
            
            if joint_angles is not None:
                response.solution.joint_state.position = joint_angles
                response.error_code.val = MoveItErrorCodes.SUCCESS
            else:
                response.error_code.val = MoveItErrorCodes.NO_IK_SOLUTION
                
        except Exception as e:
            response.error_code.val = MoveItErrorCodes.FAILURE
            
        return response
```

#### 碰撞检测服务
```python
class CollisionDetectionService:
    def __init__(self, planning_scene):
        self.planning_scene = planning_scene
        self.collision_detector = CollisionDetector()
        
    def check_collision_callback(self, request, response):
        """实时碰撞检测"""
        try:
            # 更新机器人状态
            robot_state = RobotState()
            robot_state.joint_state = request.joint_state
            
            # 检查自碰撞
            self_collision = self.planning_scene.isStateColliding(
                robot_state, group_name="", verbose=False)
            
            # 检查环境碰撞
            env_collision = self.planning_scene.isStateColliding(
                robot_state, group_name="", verbose=True)
            
            response.in_collision = self_collision or env_collision
            response.collision_type = self.get_collision_type(
                self_collision, env_collision)
            response.collision_objects = self.get_collision_objects(robot_state)
            
        except Exception as e:
            response.in_collision = True
            response.error_message = str(e)
            
        return response
    
    def get_collision_objects(self, robot_state):
        """获取碰撞对象列表"""
        collision_objects = []
        collision_result = CollisionResult()
        
        self.planning_scene.checkCollision(
            CollisionRequest(), collision_result, robot_state)
        
        for contact in collision_result.contacts:
            collision_objects.extend([contact.body_name_1, contact.body_name_2])
            
        return list(set(collision_objects))
```

### 4. 可视化系统

#### 轨迹可视化器
```csharp
public class TrajectoryVisualizer : MonoBehaviour
{
    [Header("可视化设置")]
    public Material trajectoryMaterial;
    public float lineWidth = 0.02f;
    public int trajectoryResolution = 100;
    public Color plannedColor = Color.green;
    public Color executingColor = Color.blue;
    public Color collisionColor = Color.red;
    
    private LineRenderer trajectoryLine;
    private List<Vector3> trajectoryPoints;
    
    public void VisualizeTrajectory(RobotTrajectory trajectory)
    {
        trajectoryPoints.Clear();
        
        // 插值轨迹点
        for (int i = 0; i < trajectory.joint_trajectory.points.Count; i++)
        {
            var jointAngles = trajectory.joint_trajectory.points[i].positions;
            var endEffectorPose = ForwardKinematics(jointAngles.ToArray());
            trajectoryPoints.Add(endEffectorPose.position);
        }
        
        // 更新LineRenderer
        trajectoryLine.positionCount = trajectoryPoints.Count;
        trajectoryLine.SetPositions(trajectoryPoints.ToArray());
        trajectoryLine.material.color = plannedColor;
    }
    
    public void UpdateExecutionProgress(float progress)
    {
        int currentIndex = Mathf.FloorToInt(progress * trajectoryPoints.Count);
        
        // 创建渐变效果
        Gradient gradient = new Gradient();
        GradientColorKey[] colorKeys = new GradientColorKey[2];
        colorKeys[0] = new GradientColorKey(executingColor, 0.0f);
        colorKeys[1] = new GradientColorKey(plannedColor, progress);
        gradient.colorKeys = colorKeys;
        
        trajectoryLine.colorGradient = gradient;
    }
}
```

#### 碰撞可视化器
```csharp
public class CollisionVisualizer : MonoBehaviour
{
    [Header("碰撞可视化")]
    public Material collisionMaterial;
    public Material warningMaterial;
    public float warningDistance = 0.1f;
    
    private Dictionary<string, GameObject> collisionHighlights;
    private Dictionary<string, Renderer> originalRenderers;
    
    public void HighlightCollisions(List<string> collisionObjects)
    {
        // 清除之前的高亮
        ClearHighlights();
        
        foreach (string objName in collisionObjects)
        {
            GameObject obj = GameObject.Find(objName);
            if (obj != null)
            {
                HighlightObject(obj, collisionMaterial);
            }
        }
    }
    
    public void ShowProximityWarnings(List<ProximityInfo> proximities)
    {
        foreach (var proximity in proximities)
        {
            if (proximity.distance < warningDistance)
            {
                GameObject obj = GameObject.Find(proximity.objectName);
                if (obj != null)
                {
                    HighlightObject(obj, warningMaterial);
                }
            }
        }
    }
    
    private void HighlightObject(GameObject obj, Material material)
    {
        Renderer renderer = obj.GetComponent<Renderer>();
        if (renderer != null)
        {
            if (!originalRenderers.ContainsKey(obj.name))
            {
                originalRenderers[obj.name] = renderer;
            }
            renderer.material = material;
        }
    }
}
```

### 5. 动态UI系统

#### 自适应控制面板
```csharp
public class AdaptiveControlPanel : MonoBehaviour
{
    [Header("UI组件")]
    public Transform jointControlsParent;
    public GameObject jointSliderPrefab;
    public Text robotInfoText;
    public Button planButton;
    public Button executeButton;
    public Button stopButton;
    
    private List<JointSlider> jointSliders;
    private UniversalRobotAdapter robotAdapter;
    
    public void InitializeForRobot(UniversalRobotAdapter adapter)
    {
        robotAdapter = adapter;
        CreateJointControls();
        UpdateRobotInfo();
        SetupEventHandlers();
    }
    
    private void CreateJointControls()
    {
        // 清除现有控件
        foreach (Transform child in jointControlsParent)
        {
            DestroyImmediate(child.gameObject);
        }
        
        jointSliders = new List<JointSlider>();
        
        // 为每个关节创建控制滑块
        for (int i = 0; i < robotAdapter.degreesOfFreedom; i++)
        {
            GameObject sliderObj = Instantiate(jointSliderPrefab, jointControlsParent);
            JointSlider slider = sliderObj.GetComponent<JointSlider>();
            
            slider.Initialize(
                jointName: $"Joint {i + 1}",
                minValue: robotAdapter.jointLimitsMin[i],
                maxValue: robotAdapter.jointLimitsMax[i],
                currentValue: 0.0f,
                onValueChanged: (value) => OnJointValueChanged(i, value)
            );
            
            jointSliders.Add(slider);
        }
    }
    
    private void UpdateRobotInfo()
    {
        robotInfoText.text = $"机械臂信息:\n" +
                           $"自由度: {robotAdapter.degreesOfFreedom}\n" +
                           $"类型: {robotAdapter.robotType}\n" +
                           $"工作空间: {robotAdapter.workspaceRadius:F2}m";
    }
}
```

## 消息定义

### 核心消息类型
```yaml
# 通用机械臂配置
RobotConfiguration:
  string robot_name
  int32 degrees_of_freedom
  string[] joint_names
  int32[] joint_types  # 0=revolute, 1=prismatic, 2=fixed
  float64[] joint_limits_min
  float64[] joint_limits_max
  float64[] joint_velocity_limits
  float64[] joint_acceleration_limits
  string urdf_path
  string srdf_path

# 交互式规划请求
InteractivePlanRequest:
  Header header
  string group_name
  geometry_msgs/PoseStamped target_pose
  sensor_msgs/JointState start_state
  float64 planning_time
  bool avoid_collisions
  string planner_id
  float64 velocity_scaling_factor
  float64 acceleration_scaling_factor

# 规划响应
InteractivePlanResponse:
  bool success
  string error_message
  moveit_msgs/RobotTrajectory trajectory
  float64 planning_time
  float64 trajectory_duration
  int32 waypoint_count

# 实时状态更新
RobotStateUpdate:
  Header header
  sensor_msgs/JointState joint_state
  geometry_msgs/PoseStamped end_effector_pose
  bool in_collision
  string[] collision_objects
  float64[] joint_velocities
  float64[] joint_efforts

# 碰撞检测请求
CollisionCheckRequest:
  sensor_msgs/JointState joint_state
  bool check_self_collision
  bool check_environment_collision
  float64 proximity_threshold

# 碰撞检测响应
CollisionCheckResponse:
  bool in_collision
  bool self_collision
  bool environment_collision
  string[] collision_pairs
  geometry_msgs/Point[] collision_points
  float64[] collision_distances
```

### 服务接口定义
```yaml
# 服务列表
services:
  - name: "/initialize_robot"
    type: "InitializeRobot"
    description: "初始化机械臂配置"
    
  - name: "/plan_to_target"
    type: "PlanToTarget" 
    description: "规划到目标位姿"
    
  - name: "/compute_ik"
    type: "ComputeIK"
    description: "计算逆运动学"
    
  - name: "/check_collision"
    type: "CheckCollision"
    description: "碰撞检测"
    
  - name: "/execute_trajectory"
    type: "ExecuteTrajectory"
    description: "执行轨迹"
    
  - name: "/get_robot_state"
    type: "GetRobotState"
    description: "获取机器人状态"

# 话题列表
topics:
  - name: "/joint_states"
    type: "sensor_msgs/JointState"
    description: "关节状态发布"
    
  - name: "/robot_state_update"
    type: "RobotStateUpdate"
    description: "机器人状态更新"
    
  - name: "/trajectory_execution_status"
    type: "ExecutionStatus"
    description: "轨迹执行状态"
    
  - name: "/collision_status"
    type: "CollisionStatus"
    description: "碰撞状态"
```

## 部署配置

### Docker配置
```yaml
# docker-compose.yml
version: '3.8'

services:
  ros2_backend:
    build: 
      context: ./ros2_workspace
      dockerfile: Dockerfile
    container_name: universal_manipulator_backend
    environment:
      - ROS_DOMAIN_ID=42
      - DISPLAY=${DISPLAY}
    volumes:
      - ./ros2_workspace:/workspace
      - ./robot_configs:/workspace/robot_configs
    ports:
      - "10000:10000"  # ROS-TCP-Endpoint
      - "11311:11311"  # ROS Master (if needed)
    networks:
      - manipulator_network
    command: >
      bash -c "
        source /opt/ros/jazzy/setup.bash &&
        source /workspace/install/setup.bash &&
        ros2 launch universal_manipulator_bringup system.launch.py
      "      

  unity_frontend:
    image: unity_manipulator_frontend:latest
    container_name: universal_manipulator_frontend
    environment:
      - ROS_IP=ros2_backend
      - ROS_PORT=10000
    ports:
      - "7777:7777"  # Unity WebGL
    networks:
      - manipulator_network
    depends_on:
      - ros2_backend

networks:
  manipulator_network:
    driver: bridge
```

### 启动配置
```xml
<!-- system.launch.py -->
<launch>
  <!-- 参数配置 -->
  <arg name="robot_config" default="$(find-pkg-share universal_manipulator_config)/config/default_robot.yaml"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="debug" default="false"/>
  
  <!-- 机器人描述 -->
  <node pkg="robot_state_publisher" exec="robot_state_publisher">
    <param name="robot_description" value="$(command 'xacro $(var robot_config)')"/>
    <param name="use_sim_time" value="$(var use_sim_time)"/>
  </node>
  
  <!-- MoveIt配置 -->
  <include file="$(find-pkg-share universal_manipulator_moveit_config)/launch/move_group.launch.py">
    <arg name="robot_config" value="$(var robot_config)"/>
    <arg name="use_sim_time" value="$(var use_sim_time)"/>
  </include>
  
  <!-- 通用规划服务 -->
  <node pkg="universal_manipulator_planning" exec="planning_service">
    <param name="robot_config" value="$(var robot_config)"/>
    <remap from="/joint_states" to="/robot/joint_states"/>
  </node>
  
  <!-- ROS-TCP端点 -->
  <node pkg="ros_tcp_endpoint" exec="default_server_endpoint">
    <param name="ROS_IP" value="0.0.0.0"/>
    <param name="ROS_TCP_PORT" value="10000"/>
  </node>
  
  <!-- Unity控制示例 -->
  <node pkg="unity_control_example" exec="follow_joint_trajectory_monitor">
    <remap from="/joint_trajectory_topic" to="/robot/joint_trajectory"/>
  </node>
  
</launch>
```

## 使用指南

### 1. 系统初始化
```bash
# 启动Docker容器
docker-compose up -d

# 检查服务状态
docker-compose ps

# 查看日志
docker-compose logs -f ros2_backend
```

### 2. 机械臂配置
```yaml
# robot_config.yaml 示例
robot_name: "universal_manipulator"
degrees_of_freedom: 7
joint_names: ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"]
joint_types: [0, 0, 0, 0, 0, 0, 0]  # 全部为旋转关节
joint_limits_min: [-3.14, -1.57, -3.14, -1.57, -3.14, -1.57, -3.14]
joint_limits_max: [3.14, 1.57, 3.14, 1.57, 3.14, 1.57, 3.14]
joint_velocity_limits: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
joint_acceleration_limits: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0]
urdf_path: "package://robot_description/urdf/robot.urdf.xacro"
srdf_path: "package://robot_description/srdf/robot.srdf"
```

### 3. Unity操作指南
1. **启动Unity项目**：打开Unity编辑器，加载项目
2. **配置机械臂**：在Inspector中设置机械臂参数
3. **连接ROS2**：确保ROS连接状态正常
4. **交互操作**：
   - 左键拖拽：移动末端执行器位置
   - 右键拖拽：旋转末端执行器姿态
   - 滚轮：缩放视图
   - 中键：旋转视角

### 4. API使用示例
```python
# Python客户端示例
import rclpy
from rclpy.node import Node
from universal_manipulator_msgs.srv import PlanToTarget
from geometry_msgs.msg import PoseStamped

class ManipulatorClient(Node):
    def __init__(self):
        super().__init__('manipulator_client')
        self.client = self.create_client(PlanToTarget, '/plan_to_target')
        
    def plan_to_pose(self, target_pose):
        request = PlanToTarget.Request()
        request.target_pose = target_pose
        request.planning_time = 5.0
        request.avoid_collisions = True
        
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        
        return future.result()

# 使用示例
def main():
    rclpy.init()
    client = ManipulatorClient()
    
    # 创建目标位姿
    target_pose = PoseStamped()
    target_pose.header.frame_id = "base_link"
    target_pose.pose.position.x = 0.5
    target_pose.pose.position.y = 0.0
    target_pose.pose.position.z = 0.5
    target_pose.pose.orientation.w = 1.0
    
    # 执行规划
    result = client.plan_to_pose(target_pose)
    
    if result.success:
        print(f"规划成功！轨迹包含 {result.waypoint_count} 个路径点")
    else:
        print(f"规划失败：{result.error_message}")
    
    rclpy.shutdown()

if __name__ == '__main__':
    main()
```

## 扩展开发

### 添加新的机械臂类型
1. **创建URDF模型**：定义机械臂的几何和运动学结构
2. **配置MoveIt**：生成SRDF和规划配置文件
3. **更新Unity预制件**：创建对应的Unity机械臂模型
4. **测试验证**：确保所有功能正常工作

### 自定义规划算法
```python
class CustomPlanner:
    def __init__(self, robot_model):
        self.robot_model = robot_model
        
    def plan(self, start_state, goal_state, constraints=None):
        # 实现自定义规划算法
        pass
        
    def optimize_trajectory(self, trajectory):
        # 轨迹优化
        pass
```

### 添加新的交互方式
```csharp
public class VoiceController : MonoBehaviour
{
    public void OnVoiceCommand(string command)
    {
        switch (command.ToLower())
        {
            case "move to home":
                MoveToHomePosition();
                break;
            case "plan trajectory":
                PlanCurrentTrajectory();
                break;
            // 添加更多语音命令
        }
    }
}
```

## 故障排除

### 常见问题
1. **连接问题**：检查ROS_IP和端口配置
2. **规划失败**：检查关节限制和碰撞设置
3. **性能问题**：调整规划参数和可视化设置
4. **URDF错误**：验证机械臂模型的正确性

### 调试工具
- **ROS2话题监控**：`ros2 topic echo /joint_states`
- **服务测试**：`ros2 service call /plan_to_target`
- **Unity控制台**：查看Unity日志输出
- **MoveIt可视化**：使用RViz进行调试

### 性能优化
- **减少碰撞检测频率**：在不需要时禁用实时碰撞检测
- **优化轨迹分辨率**：根据需要调整轨迹点密度
- **使用LOD模型**：为复杂模型提供多级细节
- **异步处理**：将耗时操作移到后台线程

## 总结

本系统提供了一个完整的Unity-MoveIt2集成解决方案，支持1-9自由度的通用机械臂仿真。通过模块化设计和标准化接口，系统具有良好的扩展性和可维护性。用户可以根据具体需求进行定制和扩展，实现各种复杂的机械臂应用场景。

---

## 附录：缺失项清单与优化建议

为提升文档完整性与可实施性，建议补充以下关键部分（已在下文逐项给出内容与建议）：
- 架构数据流与时序说明（交互→IK→规划→执行→反馈）
- ROS2通信细节与QoS策略（topic/service QoS 选择与参数）
- 坐标系与单位规范（frame 命名、单位换算、方向约定）
- 失败处理与回退策略（IK/规划失败、碰撞、超时、网络中断）
- 性能与实时性调优（延迟预算、频率、线程与异步）
- 测试与验证策略（单元、集成、仿真对比与基准测试）
- 多自由度适配指南（1-9 DOF 动态映射、规划组与关节名）
- 模型配置与工具链（URDF/SRDF 生成、MoveIt Setup Assistant）
- 部署与环境细化（Windows/Ubuntu/Docker 的详细步骤与差异）
- 日志与监控（ROS2/Unity 统一日志、指标与告警）
- 版本与CI/CD（语义化版本、构建流水线与发布规范）
- 安全与网络（端口、域 ID、局域网与容器网络安全）
- 术语与约定（统一术语表，降低沟通与实现歧义）

---

## 架构数据流与时序

交互拖拽一次完整流程的时序说明：
1. 用户在 Unity 拖拽末端执行器目标位姿 `T_goal`（包含位置与姿态）。
2. Unity 进行局部约束预检（工作空间边界、基本关节限制）。
3. Unity 发起 IK 请求（可选）或直接发起规划请求到 MoveIt2：
   - IK（可选）：用于快速给出初始/预览关节解；
   - 规划：`/plan_to_target` 使用 `start_state` 与 `T_goal` 生成轨迹。
4. MoveIt2 执行碰撞检查、路径搜索与时间参数化，返回 `RobotTrajectory`。
5. Unity 可视化轨迹（预览），用户确认后发起 `/execute_trajectory`。
6. MoveIt2 执行轨迹，周期性发布执行状态与当前 `JointState`。
7. Unity 根据反馈更新渲染、进度与碰撞提示，结束后汇报结果。

关键数据：
- 输入：`PoseStamped T_goal`、`JointState start_state`、`planning_group`；
- 输出：`RobotTrajectory`、成功标志、规划时间、错误说明；
- 反馈：执行进度、当前位置/姿态、碰撞状态（若启用在线检查）。

---

## ROS2 通信细节与 QoS 策略

推荐 QoS（根据实时性与可靠性需求）：
- `/joint_states`：`reliability=Reliable`，`history=KeepLast(depth=10)`；
- `/robot_state_update`：`reliability=Reliable`，`history=KeepLast(depth=10)`；
- `/trajectory_execution_status`：`reliability=Reliable`，`history=KeepLast(depth=5)`；
- 规划与服务接口：使用服务的默认可靠传输；
- 大量状态流（如传感器）可选择 `BestEffort` 以减小延迟与拥塞。

网络与域配置：
- `ROS_DOMAIN_ID` 建议在 `20-50` 范围内，为项目专用隔离；
- 容器网络使用 `bridge` 并显式暴露 ROS-TCP 端口（默认 `10000`）；
- 跨平台通信（Windows↔Linux）需保证防火墙放通相应端口与多播。

---

## 坐标系与单位规范

- 统一使用米（m）、弧度（rad）；不混用角度与弧度。
- 基准坐标系：`base_link`（右手坐标系，Z 向上，X 前，Y 左）
- 末端执行器：`tool0` 或 `ee_link`，在 SRDF/URDF 中明确工具中心点（TCP）。
- Unity↔ROS 坐标转换：明确 ROS 与 Unity 的轴向约定并封装转换函数：
  - 保证四元数归一化与欧拉角范围一致；
  - 明确世界坐标与机器人基座局部坐标的转换关系。

---

## 失败处理与回退策略

- IK 失败：
  - 提示不可达（颜色/文案），建议调整姿态或放宽约束；
  - 自动切换为全局规划尝试，记录失败原因与迭代次数。
- 规划失败：
  - 提供替代方案（增加规划时间、变换规划器、增加中间路点）；
  - 支持用户交互插入 waypoint 进行分段规划。
- 碰撞风险：
  - 高亮碰撞对与近邻对象，提示最小安全距离；
  - 支持快速重新规划与安全停机。
- 超时与网络中断：
  - 服务调用设置超时与重试策略，断线自动重连与状态恢复。

---

## 性能与实时性调优

- 延迟预算：
  - 前端交互采样：`60–120 Hz`；ROS 状态同步：`30–60 Hz`；
  - 规划响应：简单场景 `<200 ms`，复杂场景 `<500 ms` 优化目标。
- 线程与异步：
  - Unity 侧将 ROS 调用与重计算放到后台线程；
  - 主线程仅负责交互与渲染；避免阻塞 UI。
- 规划参数：
  - `planning_time`、`num_attempts`、`ompl` planner 切换与 seed 优化；
  - 时间参数化（TOPP/IterativeParabolic）权衡平滑与时效。

---

## 测试与验证策略

- 单元测试：
  - 逆运动学接口、坐标转换、消息序列化与约束检查。
- 集成测试：
  - 交互拖拽→规划→执行的端到端测试，含碰撞场景用例。
- 仿真对比：
  - 与 RViz2 的 MoveIt 规划结果进行一致性与时间对比。
- 基准测试：
  - 各 DOF、不同场景复杂度下的响应时间与成功率统计。

---

## 多自由度适配指南（1–9 DOF）

- 动态映射：根据 `RobotConfiguration` 自动生成 `joint_names`、`planning_group`、控制器数组与 UI 控件。
- 规划组选择：
  - 串联臂：`manipulator`；带手爪：`gripper` 分组；
  - 混合结构：明确主运动链与辅助链的组名与约束。
- 关节限制：
  - 从 URDF/SRDF 读入限制并在 Unity 控制器中强制执行。

---

## 模型配置与工具链

- 使用 MoveIt Setup Assistant：
  - 导入 URDF，生成 SRDF；配置规划组、虚拟关节与碰撞矩阵；
  - 导出 `moveit_config` 包并纳入工作空间。
- URDF 规范：
  - 统一惯性、质量、关节类型与限制；避免非标准标签与缩放不一致。

---

## 部署与环境细化

- Windows（Unity 开发）
  - 安装 Unity 2022.3 LTS 与 ROS-TCP-Connector；
  - 确认 `ROS_IP`、端口与防火墙放行；
  - 使用本地或容器化的 ROS2 后端。
- Ubuntu（ROS2/MoveIt2）
  - 安装 ROS2 Jazzy 与 MoveIt2；
  - 通过 Docker 统一依赖与版本；启用 `--network host` 或 bridge。
- GPU/渲染：
  - 若使用 WebGL/远程显示，考虑压缩与帧率限制。

---

## 日志与监控

- 统一日志：
  - ROS2：`rclcpp/rclpy` 日志级别（DEBUG/INFO/WARN/ERROR）；
  - Unity：分类日志（通信、规划、渲染），可切换级别。
- 指标与告警：
  - 规划耗时、成功率、碰撞事件、帧率、丢包率；
  - 可选接入 Prometheus（容器内）与可视化面板。

---

## 版本与 CI/CD

- 语义化版本（SemVer）：`MAJOR.MINOR.PATCH`，接口变更需升级 MAJOR。
- 构建流水线：
  - ROS2 包编译与测试；Unity 构建（Standalone/WebGL）；
  - Docker 镜像构建与推送；端到端集成测试。

---

## 安全与网络

- 端口与访问控制：限制对外暴露端口，仅允许可信网段；
- 容器权限最小化：只挂载必要卷与 Capabilities；
- 记录与审计：关键操作与故障事件留痕。

---

## 术语与约定（简表）

- DOF：自由度；EE：末端执行器；TCP：工具中心点；
- IK：逆运动学；FK：正运动学；OMPL：开源运动规划库；
- SRDF/URDF：机器人语义与描述文件；QoS：服务质量配置；
- `base_link`/`tool0`：机器人基座与工具坐标系。
## 面向有限空间装配的可行性分析扩展

本章节在现有系统设计基础上，围绕“机械臂在有限空间执行装配任务的可行性判断与环境影响识别”进行优化与补充，提供统一的任务/环境建模、可行性评估流程、布局优化与可视化交互方案。

### 目标与问题定义
- 在受限空间（机柜、工位夹具、邻近障碍）中执行特定装配任务（插接、拧紧、定位），评估是否可达、可操作且安全。
- 输出“可行区域”“不可行区域”“边界区域”与影响因素（遮挡、碰撞、空间不足、姿态受限）。
- 给出建议的机器人基座位置、装配工位物体布局与操作路径。

### 任务与环境建模
- 任务目标：以 `TargetPose[]` 或 `TargetFeature[]` 表示装配位姿/特征（孔、面、边），支持容差与插入方向约束（例如允许的姿态偏差、插入轴方向）。
- 操作约束：工具长度、末端执行器几何、必要的进出路径约束（直线插入、保持法向、避免旋转），以及扭力/力控需求标签。
- 环境元素：障碍集合（网格/网格包围盒）、夹具与治具、墙体、地面，支持语义标签（可移动/不可移动）。
- 机器人配置：`URDF/SRDF` 与 `MoveIt2` Planning Group；关节限位、速度/加速度限值、可切换末端工具姿态约束。

### 可行性分析模块
- 可达性与任务覆盖：对目标位姿进行 IK 采样与可达性检查，统计可达比例、姿态可用区间、目标容差满足度。
- 碰撞与间隙：使用 `FCL` 最小距离查询与碰撞检测，输出最小安全间隙分布与危险接触对。
- 操作可操作性：计算 `Yoshikawa` 操作度、条件数、末端姿态约束下的可操作域，识别操作困难姿态。
- 路径可行性：基于 `RRT*/PRM` 检查从进入路径到目标的整段路径是否存在，包含进出策略（直进直出/分段退避）。
- 环境敏感性：对可移动障碍进行参数扫描（位置/尺寸微调），分析对可行比例与间隙的影响，输出敏感性热图。

### 布局优化模块
- 基座位姿优化：在候选基座位姿网格/采样空间中搜索最大任务覆盖与最小碰撞率的配置。
- 夹具位置优化：对可移动治具进行位置/朝向参数扫描，寻找满足可达性与路径可行性的最优配置。
- 多目标优化：综合权重（可达性、路径长度、间隙、操作度、切换成本）进行 Pareto 搜索与推荐。

### Unity交互与可视化扩展
- 可行性热图叠加：在场景中以体素/网格形式显示“可达/不可达/边界”与最小间隙值热图。
- 交互式环境编辑：在编辑面板中拖拽/启用/禁用障碍与治具，实时触发增量评估。
- 路径与姿态视图：显示插入路径、末端姿态锥体（插入轴与容差），以及碰撞近邻对的连线。
- 布局建议：根据优化结果渲染推荐的基座位置与治具姿态，可一键应用并回滚。

### ROS2接口扩展定义
- Service `EvaluateWorkspace`：输入任务与环境配置，输出覆盖率、间隙统计、不可行原因分布与热图数据句柄。
- Service `OptimizeBasePose`：输入候选区域与权重，输出推荐的基座位姿与评分构成。
- Service `SetEnvironmentObjects`：增量更新障碍/治具集合，触发缓存刷新与增量评估。
- Topic `feasibility/heatmap`：热图块/体素数据发布，Unity订阅并渲染。
- Topic `feasibility/diagnostics`：发布不可行原因聚合（遮挡、空间不足、姿态受限、路径失败）。

示例消息（伪定义）：
```
srv EvaluateWorkspace.srv
---
float32 coverage_ratio
float32 min_clearance
FeasibilityReasonCount[] reasons
HeatmapHandle heatmap
```

### 工作流程（建议）
- 场景搭建：导入机器人与工位模型，标注装配目标与约束。
- 初步评估：运行 `EvaluateWorkspace` 得到可行性热图与原因分布。
- 优化布局：调用 `OptimizeBasePose` 与环境参数扫描，应用建议并二次评估。
- 路径验证：对关键目标运行规划与执行仿真，检查碰撞与操作度。
- 报告输出：生成可行区域截图、统计表与建议配置，归档配置与结果。

### 配置样例（YAML）
```
task:
  targets:
    - pose: [0.45, 0.12, 0.30, 0, 0, 1.57]
      tolerance:
        position_mm: 2.0
        orientation_deg: 2.0
      insert_axis: [0, 0, 1]
      insert_rule: straight_in_out
  tool:
    length_mm: 120
    geometry: cylinder_ø20
environment:
  obstacles:
    - type: box
      size: [0.3, 0.2, 0.4]
      pose: [0.2, -0.1, 0.2, 0, 0, 0]
      movable: true
robot:
  planning_group: manipulator
  end_effector: ee_link
optimization:
  base_pose_search:
    region: [[-0.3, 0.3], [-0.3, 0.3], [0, 0]]
    resolution: 0.05
  weights:
    coverage: 0.5
    clearance: 0.3
    manipulability: 0.2
```

### 性能与缓存策略
- 分层评估：先做 IK 可达性与粗粒度碰撞过滤，再做路径与操作度精评估。
- 增量缓存：环境微调后仅在受影响体素/区域重新评估，支持热图块级刷新。
- 批处理与离线：大规模基座/夹具搜索采用离线批处理，结果持久化以便前端快速加载。

### 测试与评估
- 单目标可达性测试：不同障碍布局下覆盖率与最小间隙对比。
- 进出路径可靠性：插入与退出路径在不同容差下成功率统计。
- 操作度边界：识别低操作度导致失败的典型姿态并给出规避建议。
- 用户交互一致性：编辑器中环境调整与评估结果的同步与响应时间。

以上扩展与接口与现有系统架构完全兼容，并聚焦装配任务在有限空间的可行性判断与环境影响识别，帮助快速确定“在哪些位置可以完成装配”与“哪些环境因素需要调整”。