一、简介

trajectory类提供写入轨迹路点、控制轨迹运动、注册轨迹运动结果回调等功能，通过此类可以实现类似于movel和movej运动

二、操作流程

1、安装elite python sdk，确保sdk安装正确、解释器使用python版本正确

2、进入示例代码所在文件夹，打开cmd

3、执行这行代码：python example_trajectory.py --ip 192.168.0.1 ，请注意，这里example_trajectory.py是文件名，需要替换成真实文件名，--ip参数后输入的是机器人的ip地址，也需要根据实际ip输入

三、常见问题

1、Q:为什么sdk安装后，ide却提示找不到这个库呢？

A:首先请确认安装的python wheel基于什么版本编译的，和使用的版本是否有差异，其次，需要确认当前环境变量里优先级最高的python版本和pip版本，与ide中的解释器使用的是否是同一版本，如果不是可以修改环境变量后重新安装sdk，也可以修改解释器版本

2、只能通过控制台执行这个文件吗？能否通过ide直接执行？

A:这里使用了argparse解析参数，所以只需要将这里参数解析部分删除，直接指定ip, port即可

四、代码和附件

解析参数 --ip（必传）、--local_ip（可选）、--use_headless_mode（必传）
RtsiIOInterface("resource/output_recipe.txt", "resource/input_recipe.txt", 250.0) 构造 RTSI 接口，读配方文件，250Hz
rtsi.connect(ip) 连 RTSI
rtsi.getActualJointPositions() 读当前关节角，改 J4 为 -1.57 rad 作为目标
EliteDriverConfig() 配置 IP、脚本路径、headless 模式
TrajectoryControl(config) 构造 → 内部 DashboardClientInterface().connect(ip) 连 Dashboard
startControl() → dashboard.powerOn() 上电 → dashboard.brakeRelease() 释放抱闸 → headless 模式走 driver.sendExternalControlScript()，否则走 dashboard.playProgram() → 轮询 driver.isRobotConnected() 等外部控制就绪
controller.moveTo(target_joints, 3, False) 关节空间运动到目标 → 内部 writeTrajectoryControlAction(START) → writeTrajectoryPoint(joints) → writeTrajectoryControlAction(NOOP) 保活 → setTrajectoryResultCallback 用 Future 异步等完成 → writeIdle()
rtsi.getActualTCPPose() 读当前 TCP 位姿，拼三段笛卡尔轨迹：[z-0.2] → [y-0.2] → [y+0.2, z+0.2]
controller.moveTrajectory(trajectory, 3, 0, True) 笛卡尔空间走三段轨迹
析构时 dashboard.disconnect() + driver.stopControl() 断开

#!/usr/bin/env python3
"""
Example script for using the elite_cs_sdk EliteDriver.

中文说明：
这个示例演示如何启动外部控制，先执行一次关节运动，
再执行一段笛卡尔轨迹运动。

Usage:
    python example_trajectory.py --ip 192.168.0.58

**Note (Very Important):** This script will move the robot to the joint position [0, 0, 0, 0, 0, 0]. Please ensure the safety of the operating environment.
"""

import argparse
import sys
import elite_cs_sdk as cs
import concurrent.futures
import time
import os
import inspect


def get_package_installation_path(package_name):
    # 获取 SDK 安装目录，用于定位 external_control.script。
    module = sys.modules.get(package_name)
    if module and hasattr(module, '__file__'):
        return os.path.dirname(os.path.abspath(module.__file__))
    return None


def currentFile():
    return inspect.currentframe().f_code.co_filename


def currentLine():
    return inspect.currentframe().f_back.f_lineno


class TrajectoryControl:
    def __init__(self, EliteDriverConfig: cs.EliteDriverConfig):
        self.__config = EliteDriverConfig
        self.__dashboard = cs.DashboardClientInterface()
        self.__driver = cs.EliteDriver(EliteDriverConfig)

        cs.logInfoMessage(currentFile(), currentLine(), "Connecting to the dashboard")
        if not self.__dashboard.connect(self.__config.robot_ip):
            cs.logFatalMessage(currentFile(), currentLine(), "Failed to connect to the dashboard.")
            raise "Failed to connect to the dashboard."
        cs.logInfoMessage(currentFile(), currentLine(), "Successfully connected to the dashboard")

    def __del__(self):
        self.__dashboard.disconnect()
        self.__driver.stopControl()

    def startControl(self) -> bool:
        # 机器人上电、释放刹车，并启动外部控制脚本。
        cs.logInfoMessage(currentFile(), currentLine(), "Start powering on...")
        if not self.__dashboard.powerOn():
            cs.logFatalMessage(currentFile(), currentLine(), "Power-on failed")
            return False
        cs.logInfoMessage(currentFile(), currentLine(), "Power-on succeeded")

        cs.logInfoMessage(currentFile(), currentLine(), "Start releasing brake...")
        if not self.__dashboard.brakeRelease():
            cs.logFatalMessage(currentFile(), currentLine(), "Brake release failed")
            return False
        cs.logInfoMessage(currentFile(), currentLine(), "Brake released")

        if self.__config.headless_mode:
            if not self.__driver.isRobotConnected():
                if not self.__driver.sendExternalControlScript():
                    cs.logFatalMessage(currentFile(), currentLine(), "Fail to send external control script")
                    return False
        else:
            if not self.__dashboard.playProgram():
                cs.logFatalMessage(currentFile(), currentLine(), "Fail to play program")
                return False

        cs.logInfoMessage(currentFile(), currentLine(), "Wait external control script run...")
        while not self.__driver.isRobotConnected():
            time.sleep(0.1)
        cs.logInfoMessage(currentFile(), currentLine(), "External control script is running")
        return True

    def moveTrajectory(self, target_points: list[list[float]], point_time: float, blend_radius: float, is_cartesian: bool) -> bool:
        # 使用 Future 等待轨迹结束回调，避免阻塞式忙等。
        move_done_future = concurrent.futures.Future()

        def trajectory_result_callback(result: cs.TrajectoryMotionResult):
            if not move_done_future.done():
                move_done_future.set_result(result)

        self.__driver.setTrajectoryResultCallback(trajectory_result_callback)

        cs.logInfoMessage(currentFile(), currentLine(), "Trajectory motion start")
        if not self.__driver.writeTrajectoryControlAction(cs.TrajectoryControlAction.START, len(target_points), 200):
            cs.logFatalMessage(currentFile(), currentLine(), "Failed to start trajectory motion")
            return False

        for joints in target_points:
            # 逐点下发轨迹点，每个点可指定持续时间和混合半径。
            if not self.__driver.writeTrajectoryPoint(joints, point_time, blend_radius, is_cartesian):
                cs.logFatalMessage(currentFile(), currentLine(), "Failed to write trajectory point")
                return False
            # 持续发送 NOOP，避免轨迹控制超时。
            if not self.__driver.writeTrajectoryControlAction(cs.TrajectoryControlAction.NOOP, 0, 200):
                cs.logFatalMessage(currentFile(), currentLine(), "Failed to send NOOP command")
                return False

        # 等待轨迹完成期间，也需要持续发送 NOOP 保活。
        while not move_done_future.done():
            time.sleep(0.01)
            if not self.__driver.writeTrajectoryControlAction(cs.TrajectoryControlAction.NOOP, 0, 200):
                cs.logFatalMessage(currentFile(), currentLine(), "Failed to send NOOP command")
                return False

        result = move_done_future.result()
        cs.logInfoMessage(currentFile(), currentLine(), f"Trajectory motion completed with result: {result}")

        if not self.__driver.writeIdle(0):
            cs.logFatalMessage(currentFile(), currentLine(), "Failed to write idle command")
            return False

        return result == cs.TrajectoryMotionResult.SUCCESS

    def moveTo(self, point: list[float], time: float, is_cartesian: bool) -> bool:
        return self.moveTrajectory([point], time, 0, is_cartesian)


def main():
    parser = argparse.ArgumentParser(
        description="Connect to a robot's server and perform basic operations."
    )
    parser.add_argument(
        "--ip",
        required=True,
        help="IP address of the robot's server"
    )
    parser.add_argument(
        "--local_ip",
        default="",
        help="IP address of the robot's server"
    )
    parser.add_argument(
        "--use_headless_mode",
        required=True,
        choices=["true", "false"],
        help="IP address of the robot's server"
    )
    args = parser.parse_args()

    ip, local_ip = args.ip, args.local_ip

    """
    先连接 RTSI，读取机器人当前关节角和 TCP 位姿，
    用作后续轨迹规划的起点。
    """
    rtsi = cs.RtsiIOInterface("resource/output_recipe.txt", "resource/input_recipe.txt", 250.0)
    cs.logInfoMessage(currentFile(), currentLine(), "Connecting to the RTSI...")
    if not rtsi.connect(ip):
        cs.logFatalMessage(currentFile(), currentLine(), f"Can't connect {ip} RTSI server")
        sys.exit(1)
    cs.logInfoMessage(currentFile(), currentLine(), "Successfully connected to the RTSI")

    actual_joints = rtsi.getActualJointPositions()
    target_joints = actual_joints
    # 示例中让第 4 轴转到约 -90 度。
    target_joints[3] = -1.57

    config = cs.EliteDriverConfig()
    config.robot_ip = ip
    config.local_ip = local_ip
    config.script_file_path = get_package_installation_path("elite_cs_sdk") + "/external_control.script"
    config.headless_mode = args.use_headless_mode.lower() == "true"

    controller = TrajectoryControl(config)

    cs.logInfoMessage(currentFile(), currentLine(), "Starting trajectory control...")
    if not controller.startControl():
        cs.logFatalMessage(currentFile(), currentLine(),"Failed to start trajectory control.")
        sys.exit(1)
    cs.logInfoMessage(currentFile(), currentLine(),"Trajectory control started")

    cs.logInfoMessage(currentFile(), currentLine(), f"Moving joints to target: [{target_joints[0]}, {target_joints[1]}, {target_joints[2]}, {target_joints[3]}, {target_joints[4]}, {target_joints[5]}]")
    if not controller.moveTo(target_joints, 3, False):
        cs.logFatalMessage(currentFile(), currentLine(), "Failed to move joints to target.");
        sys.exit(1)

    target_pose = rtsi.getActualTCPPose()

    # 基于当前 TCP 位姿拼出一个简单的笛卡尔三点轨迹。
    target_pose[2] -= 0.2
    trajectory = [target_pose.copy()]

    target_pose[1] -= 0.2
    trajectory.append(target_pose.copy())

    target_pose[1] += 0.2
    target_pose[2] += 0.2
    trajectory.append(target_pose.copy())

    cs.logInfoMessage(currentFile(), currentLine(), "Moving joints to target")
    if not controller.moveTrajectory(trajectory, 3, 0, True):
        cs.logFatalMessage(currentFile(), currentLine(), "Failed to move trajectory.")
        sys.exit(1)
    cs.logInfoMessage(currentFile(), currentLine(), "Joints moved to target")


if __name__ == "__main__":
    try:
        main()
    except Exception as e:
        print(f"[ERROR] {e}", file=sys.stderr)
        sys.exit(1)

目录

首页

二次开发

Python

CS机器人 python SDK使用案例

简单案例

trajectory 的使用

trajectory 的使用

一、简介

二、操作流程

三、常见问题

四、代码和附件

目录

首页 二次开发 Python CS机器人 python SDK使用案例 简单案例 trajectory 的使用

一、简介

二、操作流程

三、常见问题

四、代码和附件

首页

二次开发

Python

CS机器人 python SDK使用案例

简单案例

trajectory 的使用