unity2moveit2/docs/api/API.md

Unity-MoveIt2 API 接口文档

📋 目录

• 1. API概览
• 2. Unity端API
• 3. ROS2端API
• 4. 消息定义
• 5. 服务接口
• 6. 话题列表
• 7. 错误码定义
• 8. 使用示例

1. API概览

1.1 通信架构

Unity前端 ←→ ROS TCP Bridge ←→ ROS2后端
    ↓              ↓              ↓
Unity API    TCP Protocol    ROS2 API

1.2 支持的通信模式

• 实时控制：高频率位姿控制（30Hz）
• 路径规划：请求-响应模式
• 状态监控：订阅-发布模式
• 配置管理：服务调用模式

2. Unity端API

2.1 机器人控制API

RobotController类

public class RobotController : MonoBehaviour
{
    /// <summary>
    /// 设置末端执行器目标位姿
    /// </summary>
    /// <param name="position">目标位置 (世界坐标系)</param>
    /// <param name="rotation">目标旋转 (四元数)</param>
    /// <returns>是否成功发送命令</returns>
    public bool SetEndEffectorPose(Vector3 position, Quaternion rotation)
    
    /// <summary>
    /// 设置关节角度
    /// </summary>
    /// <param name="jointAngles">关节角度数组 (弧度)</param>
    /// <returns>是否成功发送命令</returns>
    public bool SetJointAngles(float[] jointAngles)
    
    /// <summary>
    /// 获取当前关节状态
    /// </summary>
    /// <returns>关节状态信息</returns>
    public JointState GetCurrentJointState()
    
    /// <summary>
    /// 获取当前末端位姿
    /// </summary>
    /// <returns>末端位姿信息</returns>
    public Pose GetCurrentEndEffectorPose()
}

使用示例

// 设置末端位姿
Vector3 targetPosition = new Vector3(0.5f, 0.2f, 0.3f);
Quaternion targetRotation = Quaternion.Euler(0, 90, 0);
robotController.SetEndEffectorPose(targetPosition, targetRotation);

// 设置关节角度
float[] jointAngles = {0.0f, -1.57f, 1.57f, 0.0f, 1.57f, 0.0f};
robotController.SetJointAngles(jointAngles);

2.2 路径规划API

PathPlannerInterface类

public class PathPlannerInterface : MonoBehaviour
{
    /// <summary>
    /// 规划路径到目标位姿
    /// </summary>
    /// <param name="targetPose">目标位姿</param>
    /// <param name="constraints">路径约束</param>
    /// <param name="callback">规划完成回调</param>
    public void PlanToTarget(Pose targetPose, PlanningConstraints constraints, 
                           System.Action<PlanningResult> callback)
    
    /// <summary>
    /// 规划通过路径点的轨迹
    /// </summary>
    /// <param name="waypoints">路径点列表</param>
    /// <param name="callback">规划完成回调</param>
    public void PlanThroughWaypoints(List<Pose> waypoints, 
                                   System.Action<PlanningResult> callback)
    
    /// <summary>
    /// 执行规划的轨迹
    /// </summary>
    /// <param name="trajectory">轨迹数据</param>
    /// <param name="callback">执行完成回调</param>
    public void ExecuteTrajectory(Trajectory trajectory, 
                                System.Action<ExecutionResult> callback)
}

使用示例

// 规划路径
Pose targetPose = new Pose(new Vector3(0.6f, 0.0f, 0.4f), Quaternion.identity);
PlanningConstraints constraints = new PlanningConstraints
{
    maxVelocity = 0.5f,
    maxAcceleration = 1.0f,
    allowCollisions = false
};

pathPlanner.PlanToTarget(targetPose, constraints, (result) =>
{
    if (result.success)
    {
        Debug.Log($"规划成功，轨迹点数：{result.trajectory.points.Count}");
        pathPlanner.ExecuteTrajectory(result.trajectory, OnExecutionComplete);
    }
    else
    {
        Debug.LogError($"规划失败：{result.errorMessage}");
    }
});

2.3 可行性分析API

FeasibilityAnalyzer类

public class FeasibilityAnalyzer : MonoBehaviour
{
    /// <summary>
    /// 分析装配任务可行性
    /// </summary>
    /// <param name="assemblyTask">装配任务定义</param>
    /// <param name="callback">分析完成回调</param>
    public void AnalyzeAssemblyTask(AssemblyTask assemblyTask, 
                                  System.Action<FeasibilityResult> callback)
    
    /// <summary>
    /// 分析工作空间可达性
    /// </summary>
    /// <param name="workspace">工作空间定义</param>
    /// <param name="callback">分析完成回调</param>
    public void AnalyzeWorkspace(WorkspaceDefinition workspace, 
                               System.Action<ReachabilityResult> callback)
    
    /// <summary>
    /// 优化装配序列
    /// </summary>
    /// <param name="assemblySteps">装配步骤</param>
    /// <param name="callback">优化完成回调</param>
    public void OptimizeAssemblySequence(List<AssemblyStep> assemblySteps, 
                                       System.Action<OptimizationResult> callback)
}

2.4 可视化API

VisualizationManager类

public class VisualizationManager : MonoBehaviour
{
    /// <summary>
    /// 显示轨迹
    /// </summary>
    /// <param name="trajectory">轨迹数据</param>
    /// <param name="color">显示颜色</param>
    public void ShowTrajectory(Trajectory trajectory, Color color)
    
    /// <summary>
    /// 显示碰撞热图
    /// </summary>
    /// <param name="collisionData">碰撞数据</param>
    public void ShowCollisionHeatmap(CollisionData collisionData)
    
    /// <summary>
    /// 显示工作空间
    /// </summary>
    /// <param name="workspace">工作空间数据</param>
    /// <param name="transparency">透明度</param>
    public void ShowWorkspace(WorkspaceData workspace, float transparency)
    
    /// <summary>
    /// 清除所有可视化
    /// </summary>
    public void ClearAllVisualizations()
}

3. ROS2端API

3.1 规划服务

服务定义

# PlanPath.srv
---
# 请求
geometry_msgs/Pose start_pose
geometry_msgs/Pose goal_pose
geometry_msgs/Pose[] waypoints
moveit_msgs/Constraints path_constraints
moveit_msgs/Constraints goal_constraints
float64 max_velocity_scaling_factor
float64 max_acceleration_scaling_factor
string planner_id
float64 planning_time
int32 planning_attempts
bool allow_replanning
---
# 响应
bool success
string error_message
moveit_msgs/RobotTrajectory trajectory
moveit_msgs/MoveItErrorCodes error_code
float64 planning_time

Python客户端示例

import rclpy
from rclpy.node import Node
from unity_moveit_msgs.srv import PlanPath
from geometry_msgs.msg import Pose, Point, Quaternion

class PlanningClient(Node):
    def __init__(self):
        super().__init__('planning_client')
        self.client = self.create_client(PlanPath, 'plan_path')
        
    def plan_to_pose(self, target_pose):
        request = PlanPath.Request()
        request.goal_pose = target_pose
        request.max_velocity_scaling_factor = 0.5
        request.max_acceleration_scaling_factor = 0.5
        request.planner_id = "RRTConnect"
        request.planning_time = 5.0
        request.planning_attempts = 10
        
        future = self.client.call_async(request)
        return future

3.2 控制话题

关节控制

# 话题：/joint_command
# 消息类型：sensor_msgs/JointState
Header header
string[] name
float64[] position
float64[] velocity
float64[] effort

末端控制

# 话题：/end_effector_command
# 消息类型：geometry_msgs/PoseStamped
Header header
Pose pose
  Point position
    float64 x
    float64 y
    float64 z
  Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

3.3 状态发布

机器人状态

class RobotStatePublisher(Node):
    def __init__(self):
        super().__init__('robot_state_publisher')
        self.joint_state_pub = self.create_publisher(
            JointState, '/joint_states', 10)
        self.ee_pose_pub = self.create_publisher(
            PoseStamped, '/end_effector_pose', 10)
        
        # 定时发布状态
        self.timer = self.create_timer(0.033, self.publish_states)  # 30Hz
        
    def publish_states(self):
        # 发布关节状态
        joint_msg = JointState()
        joint_msg.header.stamp = self.get_clock().now().to_msg()
        joint_msg.name = self.joint_names
        joint_msg.position = self.current_joint_positions
        joint_msg.velocity = self.current_joint_velocities
        self.joint_state_pub.publish(joint_msg)
        
        # 发布末端位姿
        ee_msg = PoseStamped()
        ee_msg.header.stamp = self.get_clock().now().to_msg()
        ee_msg.header.frame_id = "base_link"
        ee_msg.pose = self.current_ee_pose
        self.ee_pose_pub.publish(ee_msg)

4. 消息定义

4.1 Unity消息类型

[System.Serializable]
public class JointState
{
    public string[] jointNames;
    public float[] positions;
    public float[] velocities;
    public float[] efforts;
    public float timestamp;
}

[System.Serializable]
public class Pose
{
    public Vector3 position;
    public Quaternion orientation;
}

[System.Serializable]
public class Trajectory
{
    public TrajectoryPoint[] points;
    public float totalTime;
}

[System.Serializable]
public class TrajectoryPoint
{
    public float[] jointPositions;
    public float[] jointVelocities;
    public float timeFromStart;
}

[System.Serializable]
public class PlanningResult
{
    public bool success;
    public string errorMessage;
    public Trajectory trajectory;
    public float planningTime;
}

4.2 装配任务消息

[System.Serializable]
public class AssemblyTask
{
    public string taskName;
    public AssemblyStep[] steps;
    public CollisionObject[] environment;
    public AssemblyConstraints constraints;
}

[System.Serializable]
public class AssemblyStep
{
    public int stepId;
    public Pose targetPose;
    public string[] requiredTools;
    public float maxForce;
    public float maxTorque;
}

[System.Serializable]
public class FeasibilityResult
{
    public bool feasible;
    public float feasibilityScore;
    public CollisionRisk[] collisionRisks;
    public string[] recommendations;
}

5. 服务接口

5.1 配置管理服务

# LoadRobotConfiguration.srv
---
# 请求
string robot_name
string config_path
bool validate_config
---
# 响应
bool success
string error_message
int32 degrees_of_freedom
string[] joint_names
geometry_msgs/Pose[] joint_origins

5.2 可行性分析服务

# AnalyzeFeasibility.srv
---
# 请求
string task_name
geometry_msgs/Pose[] target_poses
shape_msgs/SolidPrimitive[] obstacles
moveit_msgs/Constraints constraints
---
# 响应
bool feasible
float64 feasibility_score
float64 collision_probability
geometry_msgs/Point[] collision_points
string[] optimization_suggestions

6. 话题列表

6.1 控制话题

话题名称	消息类型	方向	频率	描述
/joint_command	sensor_msgs/JointState	Unity→ROS2	30Hz	关节控制命令
/end_effector_command	geometry_msgs/PoseStamped	Unity→ROS2	30Hz	末端位姿命令
/velocity_command	geometry_msgs/Twist	Unity→ROS2	30Hz	速度控制命令

6.2 状态话题

话题名称	消息类型	方向	频率	描述
/joint_states	sensor_msgs/JointState	ROS2→Unity	30Hz	关节状态反馈
/end_effector_pose	geometry_msgs/PoseStamped	ROS2→Unity	30Hz	末端位姿反馈
/robot_status	unity_moveit_msgs/RobotStatus	ROS2→Unity	10Hz	机器人状态

6.3 可视化话题

话题名称	消息类型	方向	频率	描述
/trajectory_display	moveit_msgs/DisplayTrajectory	ROS2→Unity	按需	轨迹显示
/collision_objects	moveit_msgs/CollisionObject	ROS2→Unity	按需	碰撞对象
/workspace_display	unity_moveit_msgs/WorkspaceDisplay	ROS2→Unity	按需	工作空间显示

7. 错误码定义

7.1 通用错误码

public enum ErrorCode
{
    SUCCESS = 0,
    
    // 通信错误 (1000-1099)
    CONNECTION_FAILED = 1001,
    TIMEOUT = 1002,
    MESSAGE_PARSE_ERROR = 1003,
    
    // 配置错误 (1100-1199)
    INVALID_ROBOT_CONFIG = 1101,
    UNSUPPORTED_DOF = 1102,
    MISSING_URDF = 1103,
    
    // 运动学错误 (1200-1299)
    IK_SOLUTION_NOT_FOUND = 1201,
    JOINT_LIMITS_EXCEEDED = 1202,
    SINGULARITY_DETECTED = 1203,
    
    // 规划错误 (1300-1399)
    PLANNING_FAILED = 1301,
    PATH_NOT_FOUND = 1302,
    COLLISION_DETECTED = 1303,
    GOAL_UNREACHABLE = 1304,
    
    // 执行错误 (1400-1499)
    EXECUTION_FAILED = 1401,
    TRAJECTORY_INVALID = 1402,
    SAFETY_VIOLATION = 1403
}

7.2 错误处理

public class ErrorHandler
{
    public static void HandleError(ErrorCode errorCode, string context)
    {
        switch (errorCode)
        {
            case ErrorCode.CONNECTION_FAILED:
                Debug.LogError($"连接失败：{context}");
                // 尝试重连
                break;
                
            case ErrorCode.PLANNING_FAILED:
                Debug.LogWarning($"规划失败：{context}");
                // 尝试其他规划器
                break;
                
            case ErrorCode.COLLISION_DETECTED:
                Debug.LogError($"检测到碰撞：{context}");
                // 停止运动
                break;
                
            default:
                Debug.LogError($"未知错误 {errorCode}：{context}");
                break;
        }
    }
}

8. 使用示例

8.1 完整控制流程

public class RobotControlExample : MonoBehaviour
{
    public RobotController robotController;
    public PathPlannerInterface pathPlanner;
    public VisualizationManager visualizer;
    
    async void Start()
    {
        // 1. 初始化机器人
        await InitializeRobot();
        
        // 2. 规划路径
        var targetPose = new Pose(new Vector3(0.5f, 0.2f, 0.3f), Quaternion.identity);
        await PlanAndExecutePath(targetPose);
        
        // 3. 实时控制
        StartRealTimeControl();
    }
    
    async Task InitializeRobot()
    {
        // 加载机器人配置
        var config = await LoadRobotConfiguration("niryo_one");
        robotController.Initialize(config);
        
        // 移动到初始位置
        var homePosition = new float[] {0, 0, 0, 0, 0, 0};
        robotController.SetJointAngles(homePosition);
    }
    
    async Task PlanAndExecutePath(Pose targetPose)
    {
        var constraints = new PlanningConstraints
        {
            maxVelocity = 0.5f,
            maxAcceleration = 1.0f
        };
        
        var result = await pathPlanner.PlanToTargetAsync(targetPose, constraints);
        
        if (result.success)
        {
            // 显示轨迹
            visualizer.ShowTrajectory(result.trajectory, Color.green);
            
            // 执行轨迹
            await pathPlanner.ExecuteTrajectoryAsync(result.trajectory);
        }
        else
        {
            Debug.LogError($"规划失败：{result.errorMessage}");
        }
    }
    
    void StartRealTimeControl()
    {
        // 启用拖拽控制
        var dragController = GetComponent<DragController>();
        dragController.OnDrag += OnEndEffectorDrag;
    }
    
    void OnEndEffectorDrag(Vector3 position, Quaternion rotation)
    {
        robotController.SetEndEffectorPose(position, rotation);
    }
}

8.2 装配任务示例

public class AssemblyTaskExample : MonoBehaviour
{
    public FeasibilityAnalyzer analyzer;
    
    async void AnalyzeAssemblyTask()
    {
        // 定义装配任务
        var assemblyTask = new AssemblyTask
        {
            taskName = "插入螺栓",
            steps = new AssemblyStep[]
            {
                new AssemblyStep
                {
                    stepId = 1,
                    targetPose = new Pose(new Vector3(0.3f, 0.1f, 0.2f), Quaternion.identity),
                    maxForce = 10.0f
                },
                new AssemblyStep
                {
                    stepId = 2,
                    targetPose = new Pose(new Vector3(0.3f, 0.1f, 0.15f), Quaternion.identity),
                    maxForce = 50.0f
                }
            }
        };
        
        // 分析可行性
        var result = await analyzer.AnalyzeAssemblyTaskAsync(assemblyTask);
        
        if (result.feasible)
        {
            Debug.Log($"任务可行，可行性评分：{result.feasibilityScore}");
        }
        else
        {
            Debug.LogWarning("任务不可行，建议：");
            foreach (var recommendation in result.recommendations)
            {
                Debug.Log($"- {recommendation}");
            }
        }
    }
}

---

总结

本API文档详细定义了Unity-MoveIt2系统的所有接口，包括Unity端的控制API、ROS2端的服务接口、消息定义和通信协议。通过这些API，开发者可以：

1. 实现实时机器人控制：通过关节角度或末端位姿控制
2. 执行路径规划：使用MoveIt2的强大规划能力
3. 进行可行性分析：评估装配任务的可行性
4. 实现丰富的可视化：显示轨迹、碰撞、工作空间等

所有API都提供了详细的使用示例和错误处理机制，确保系统的稳定性和可靠性。