optimize_trajectory

Optimize robot trajectory for time, energy, or smoothness by adjusting waypoints to improve motion efficiency and performance.

Instructions

优化轨迹（时间、能耗或平滑度）

参数:
- ip: 机器人IP地址
- waypoints: 路径点列表
- optimization_type: 优化类型，可选值包括"time", "energy", "smoothness"

返回:
- 优化后的路径

Input Schema

TableJSON Schema

Name	Required	Default
`ip`	Yes
`waypoints`	Yes
`optimization_type`	No	time

Implementation Reference

src/nonead_universal_robots_mcp/server.py:965-1017 (handler)

MCP tool handler for optimize_trajectory: validates input, checks connection, converts waypoints, calls AdvancedTrajectoryPlanner.optimize_trajectory, formats and returns optimized path.

@mcp.tool()
def optimize_trajectory(ip: str, waypoints: list, optimization_type: str = "time"):
    """
    优化轨迹（时间、能耗或平滑度）
    
    参数:
    - ip: 机器人IP地址
    - waypoints: 路径点列表
    - optimization_type: 优化类型，可选值包括"time", "energy", "smoothness"
    
    返回:
    - 优化后的路径
    """
    try:
        if '连接失败' in link_check(ip):
            return return_msg(f"与机器人的连接已断开。")
        
        if advanced_trajectory_planner is None:
            return return_msg("高级轨迹规划器未初始化")
        
        # 转换waypoints格式
        points = [
            [p["x"], p["y"], p["z"], p["rx"], p["ry"], p["rz"]]
            for p in waypoints
        ]
        
        # 获取机器人模型参数
        robot_model = robotModle_list[ip]
        
        # 优化轨迹
        optimized_path = advanced_trajectory_planner.optimize_trajectory(
            waypoints=points,
            optimization_type=optimization_type,
            robot_model=robot_model
        )
        
        # 转换回字典格式
        result_path = []
        for point in optimized_path:
            result_path.append({
                "x": point[0],
                "y": point[1],
                "z": point[2],
                "rx": point[3],
                "ry": point[4],
                "rz": point[5]
            })
        
        return return_msg({"optimized_path": result_path})
    except Exception as e:
        logger.error(f"优化轨迹失败: {str(e)}")
        return return_msg(f"优化轨迹失败: {str(e)}")

src/nonead_universal_robots_mcp/server.py:965-965 (registration)
FastMCP tool registration decorator for optimize_trajectory.
```
@mcp.tool()
```

src/nonead_universal_robots_mcp/server.py:966-977 (schema)

Function signature with type hints and docstring defining input schema (ip: str, waypoints: list of dicts with x,y,z,rx,ry,rz, optimization_type: str default 'time') and output (dict with optimized_path).

def optimize_trajectory(ip: str, waypoints: list, optimization_type: str = "time"):
    """
    优化轨迹（时间、能耗或平滑度）
    
    参数:
    - ip: 机器人IP地址
    - waypoints: 路径点列表
    - optimization_type: 优化类型，可选值包括"time", "energy", "smoothness"
    
    返回:
    - 优化后的路径
    """

src/nonead_universal_robots_mcp/server.py:48-74 (helper)

Initializes the global advanced_trajectory_planner instance used by the optimize_trajectory tool.

def initialize_extended_modules():
    """初始化所有扩展模块"""
    global multi_robot_coordinator, advanced_trajectory_planner
    global advanced_data_recorder, advanced_data_analyzer

    try:
        # 初始化多机器人协调器
        multi_robot_coordinator = AdvancedMultiRobotCoordinator()
        logger.info("高级多机器人协调器初始化成功")

        # 初始化高级轨迹规划器
        advanced_trajectory_planner = AdvancedTrajectoryPlanner()
        logger.info("高级轨迹规划器初始化成功")

        # 初始化高级数据记录器
        advanced_data_recorder = AdvancedDataRecorder()
        logger.info("高级数据记录器初始化成功")

        # 获取数据分析器实例（单例）
        advanced_data_analyzer = get_data_analyzer()
        logger.info("高级数据分析器初始化成功")

        return True
    except Exception as e:
        logger.error(f"初始化扩展模块失败: {str(e)}")
        return False

src/nonead_universal_robots_mcp/server.py:995-1016 (helper)

Delegation to AdvancedTrajectoryPlanner.optimize_trajectory for core optimization logic.

    optimized_path = advanced_trajectory_planner.optimize_trajectory(
        waypoints=points,
        optimization_type=optimization_type,
        robot_model=robot_model
    )
    
    # 转换回字典格式
    result_path = []
    for point in optimized_path:
        result_path.append({
            "x": point[0],
            "y": point[1],
            "z": point[2],
            "rx": point[3],
            "ry": point[4],
            "rz": point[5]
        })
    
    return return_msg({"optimized_path": result_path})
except Exception as e:
    logger.error(f"优化轨迹失败: {str(e)}")
    return return_msg(f"优化轨迹失败: {str(e)}")

nUR MCP Server