TeleAvatar 开发者指南

注意：本文档专为TA1.0适配，TA2.0文档即将上线

1 快速开始

1.1 基础配置

Teleavatar二次开发版本本体内置ROS2二次开发API，用户可以根据自身需求，修改配置使机器人进入不同模式并可以通过Python或C++编写ROS2程序访问各种接口控制机器人。用户无需自行配置SDK或系统，真正实现开箱即用。

1.2 网络连接

我们推荐用户选择接入机器人自带路由器进入网络环境，详细ip配置需要登陆路由器管理界面确认 192.168.31.xx (在配套的小米路由器网络环境下)。用户可以选择无线接入或网线接入。在接入网络后，用户可以远程访问机器人的核心板（RK3588），接入方法如下

ssh root@yourRobotAddress (default passwd: root)

1.3 Docker的使用

RK3588系统内部没有安装原生ROS2环境，用户可以自行打包镜像并安装ROS2，版本为humble，ROS_DOMAIN_ID为29。

当用户将自己的Docker容器或系统内的ROS_DOMAIN_ID设置为29后将可以连接至本体ROS2系统，在此环境下用户可以访问Teleavatar内部的Topic接口。

1.4 模式配置

RK3588内部存在机器人配置文件（/root/system_config.yaml），用户可以通过修改配置文件内容，按需配置机器人进入不同的模式，再通过重启机器人（网页重启）以生效修改。

http://your3588ip:8501

robot:
  meta_mode: 1
  enable_collision_check: true
  left:
    arm_control_mode: 0
    enable: true  
  right:
    arm_control_mode: 0
    enable: true
  chassis:
    enable: true
    lift_drop_range: 0
  adjust_pose:
    right:
      - [0.0, -1.57, 0.0, -0.26, 0.0, 0.0, 0.0]
      - [0.5, -1.57, 0.0, -0.5, 0.0, 0.0, 0.0]
      - [1.0, -1.57, 0.0, -1.4, 0.0, 0.0, 0.0]
      - [1.7, -1.57, 0.0, -2.4, 0.0, 0.0, -0.68]
      - [-0.8, -0.9, 0.0, -2.3, 0.0, 0.0, -0.68]
      - [-0.8, -0.9, 0.5, -1.12, -0.38, 0.0, -0.12]

1.5 step-by-step 上手说明

1.5.1 关于副控制板的使用

副控板的使用主要包含两种情况：

1. 用户直接将程序运行在副控板上时：此时用户的节点将和主控板处于同一网络内，只需要将ROS_DOMAIN_ID设置为29,即可与主控板内部的话题互相发现。这种情况下，用户可以按照一般的ROS2机器人开发流程，通过开放的/api接口控制机器人。

1. 用户在自己的主机上运行程序时：此时如果用户的主机连接提供的小米路由器，用户的主机将与副控板在同一网络内，此时用户无法直接访问主控板，但副控板上运行了bridge服务可以让您通过副控板实现话题转发，bridge服务的配置文件可以通过配置选择哪些话题转发，哪些不需要转发，ROS_DOMAIN_ID为0，下文将对此做详细说明(1.5.3)。

1.5.2 api模式的使用步骤

用户想要体验二次开发版本的功能仅需按照如下步骤操作：

1. 配置文件的编辑：按照上述配置文件的说明，配置好您想要的模式，之后重启机器人（网页重启）即可。

1. topic列表的确认：远程登陆副控板，选择出厂提供的包含ROS2环境的容器，或者您自己的容器，将ROS_DOMAIN_ID设置为29，查看您需要的topic是否都能被发现。如发现有缺少的情况，需检查配置，网络等设置。

1. demo程序的验证：推荐先通过提供的3.5程序验证，假设您将模式配置调整为关节模式，直接运行该程序，您将看到机械臂来到一个水平位置。当然您也可以自行设置位置，具体的joint_state/target_ee_pose信息您可以在VR模式下自行示教获取。

1. 校准模式：无论您选择了何种api模式，您都可以通过校准模式测试机械臂的运动，具体使用方法见2.1.7。

1. 用户自定义控制：用户可以通过自己的模型预测位置或者提前示教好运动点，参考demo例程编写程序，完成闭环控制。

1.5.3 bridge服务的说明

如上文所述，当用户使用自己的主机进行二次开发时，主控中的topic是由bridge服务转出，而用户发送的/api相关topic也是通过bridge服务转入的。用户可以自行配置bridge服务的转入转出列表，该配置文件的目录为副控板的/rosdata/config_file/，在该目录下您将看到topic_config.yaml该文件内包含了bridge服务的转入转出topic列表，用户可以根据自己的需求在该文件中添加和删除topic信息。该服务内置在副控板的files-share-server容器中，上文提到的目录已被挂载在了此容器中，不建议用户对此容器在内的任何内置容器做删除操作，这些容器可能包含Teleavatar的基本运行构成。当用户修改了配置文件之后，同样地，需要重启files-share-server容器以生效，启动脚本的目录为/userdata/file-server/app_server.sh，重启指令为：

./app_server.sh restart

重启之后修改将生效，用户可以通过在用户机上查看topic情况验证是否生效。

关于不同类型的topic的转入转出情况，namespace为/api的控制类话题（2.1章节中介绍的接口）为转入话题，这些话题从用户主机（ROS_DOMAIN_ID为0）转入至机器人内部，状态类话题（2.2章节中介绍的接口）从机器人内部（ROS_DOMAIN_ID为29）转出至用户主机。用户机上可以看到转出话题，完整话题列表推荐在副控板上查看。

2 接口说明

2.1 控制相关TOPIC

以下是用户在二次开发中会常用到的ROS2topic，其中有用户可以直接发送的api接口，也有用户可以读取查看的关节状态topic，下面将对这些接口一一介绍。

2.1.1 /api/{$arm_side}_arm/target_pose 末端控制接口

robot:  
  meta_mode: 1  # 1代表API控制模式 用户可以在此模式发送API接口topic控制Teleavatar
  left: # 或right
    arm_control_mode: 0 # 0代表末端控制模式，至少要保证一边机械臂应用
    enable: true # 至少要保证一边机械臂可用

• 接口类型

Point position
        float64 x
        float64 y
        float64 z
Quaternion orientation
        float64 x 0
        float64 y 0
        float64 z 0
        float64 w 1

• 接口介绍

本接口为机械臂末端执行器的高级控制系统，专为Teleavatar平台设计。系统通过接收用户指定的末端目标坐标，内置逆运动学（IK）解算器自动完成从任务空间到关节空间的转换。Pose中的position所对应的为末端在base坐标系的坐标，orientation所对应的是夹爪旋转。

• 注意事项

用户层代码需要负责规划一条从“当前实际位置”到“期望目标位置”的平滑路径，并以固定的频率（50-77Hz）将路径上的点依次喂给这个接口。

2.1.2 /api/{$arm_side}_arm/joint_cmd 关节控制接口

robot:  
  meta_mode: 1  # 1代表API控制模式 用户可以在此模式发送API接口topic控制Teleavatar
  left: # 或right
    arm_control_mode: 1 # 1代表关节控制模式，至少要保证一边机械臂应用
    enable: true # 至少要保证一边机械臂可用

• 接口类型

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort

• 接口介绍

本接口提供对机械臂最底层、最直接的运动控制能力。用户通过发送精确的关节角度目标数组，Teleavatar平台将驱动各关节伺服系统，使机械臂直接运动至指定构型。该接口绕过了笛卡尔空间规划，适用于对运动轨迹有特殊定制需求或需与外部系统（如视觉伺服、力控）进行高速闭环的高级应用场景。

• 注意事项

当您使用此接口时，即表示您全权接管了机械臂的运动规划与安全责任。系统将完全信任并执行您发送的每一个关节角度指令。因此，您发送的指令序列必须是经过充分验证、在规划上可行且在物理上安全的。任何不当指令都可能导致机械臂发生非预期运动、剧烈抖动、与自身或环境发生碰撞，进而造成设备损坏、工作中断或人员安全风险。

注：推荐在50-77Hz的频率下调用

2.1.3 /api/{$arm_side}_gripper/cmd 夹爪控制接口

robot:
    meta_mode: 1 # 1代表API控制模式 用户可以在此模式发送API接口topic控制Teleavatar
left: # 或right
    arm_control_mode: 0 #或1
    enable: true #至少要保证一边机械臂可用

• 接口类型

float32 data （0, 1） #0为张到最大，1为合紧

• 接口介绍

本接口提供对夹爪的控制，用户可以通过发送float32类型的控制夹爪的力。夹爪的控制指令在程序中会以sensor_msgs/msg/JointState的形式发送给电机，但是对于夹爪我们只关心effort的值，data会经过以下计算转换为effort发送。

• 注意事项

当使用api模式时，需要同时发送机械臂控制指令，由于夹爪和机械臂共用一路CAN而在关节模式时上层程序不会向电机发送运动指令，所以需要用户层单独发送

2.1.4 /api/chassis/velocity 底盘控制接口

robot:
    meta_mode: 1 # 1代表API控制模式 用户可以在此模式发送API接口topic控制Teleavatar
chassis:
    enable: true

• 接口类型

float32[]         data          # array of data (vx, vy, rx) range (-1, 1)

• 接口介绍

此接口为底盘移动的api控制接口。用户可以在数组中按vx, vy, rx（x轴方向速度，y轴方向速度，绕z轴的角速度）的顺序依次填入速度值，程序已做归一化处理用户只需将data控制在[-1, 1]范围内即可。程序会以一定的频率输出速度，超时时间为1s，当用户超过1s没有通过api接口发送速度时，底盘会自动停止，同样的通过该接口发送为0的速度一样可以让底盘停止

2.1.5 /api/servo/cmd 升降机伺服控制接口

robot:
    meta_mode: 1 
chassis:
    enable: 1 
    lift_drop_range: 100 #需要大于0 这里举例100

• 接口类型

float32[]         data          # array of data (speed, enable)

• 接口介绍

该接口用于控制机器人的腰部升降机构（伺服电机），通过速度模式进行控制。

• 注意事项

超时保护: 如果 节点在 api_timeout (默认1秒) 内没有收到新的指令，会自动将速度归零。建议以 10Hz 或更高频率 持续发送指令。

紧急停止: 只有在 Enable 状态下才能运动，如果遇到紧急情况，发送 data[1] = 0.0 可以立即切断使能

2.1.6 /api/fsm/enable 状态机使能控制接口

robot:
    meta_mode: 1

• 接口类型

float32 data （-1, 1）

• 接口介绍

这个接口主要负责控制状态机的使能状态，以及充当心跳与安全开关的角色。

• 注意事项

用户必须以高于 1Hz (建议 10Hz-20Hz) 的频率持续发送 data=1.0。如果 FSM 超过 1 秒没有收到 data=1.0，它会认为连接断开或程序崩溃，自动触发 Force PAUSE，将机器人停下来。

2.1.7 /api/fsm/adjust 校准模式控制接口

robot:
    meta_mode: 1
left: # 或right
    arm_control_mode: 0 #或1
    enable: 1 #至少要保证一边机械臂可用

• 接口类型

float32 data （-1, 1）

• 接口介绍

该接口用于触发机器人的自动校准/复位动作。通常用于在系统启动后，将机械臂从任意位置自动移动到一个预设的“安全位置”或“举手位置”，以便后续操作。

• 注意事项

上电时，节点会从配置文件中读取 robot.adjust_pose 列表。如果没有配置，则使用代码中硬编码的默认值（一组将手臂举起的动作序列）。当所有点都执行完毕，状态机会自动切换回 PAUSE 状态。此时你可以发送 /api/fsm/enable 为 1.0 来进入 READY 状态开始正常控制。

ros2 topic pub -1 /api/fsm/adjust std_msgs/msg/Float32 '{data: 1}'
ros2 topic pub -1 /api/fsm/adjust std_msgs/msg/Float32 '{data: -1}'

2.2 状态相关TOPIC

2.2.1 /{$arm_side}_arm/joint_states 机械臂关节状态反馈

• 接口类型

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort

• 接口介绍

这个接口中的是反馈数据（Feedback）。它们报告了机械臂当前的真实物理状态（关节角度、速度、力矩）。在 API 控制闭环中，这是最重要的数据来源。

2.2.2 /{arm_side}_gripper/joint_states 夹爪状态反馈

• 接口类型

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort

• 接口介绍

这个接口中的是反馈数据（Feedback）。它们报告了夹爪当前的真实物理状态（主要关注力矩）。在 API 控制闭环中，这是最重要的数据来源。

2.2.3 /chassis/joint_states 底盘状态反馈

• 接口类型

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort

• 接口介绍

这个接口中的是反馈数据（Feedback）。它们报告了底盘当前的真实物理状态（主要关注速度）。在 API 控制闭环中，这是最重要的数据来源。

2.2.4 /{$arm_side}_arm/current_ee_pose 机械臂末端状态反馈

Point position
        float64 x
        float64 y
        float64 z
Quaternion orientation
        float64 x 0
        float64 y 0
        float64 z 0
        float64 w 1

• 接口介绍

这个接口中的是反馈数据（Feedback）。它们报告了机械臂当前的末端在笛卡尔空间中的位姿信息。在 API 控制闭环中，这是最重要的数据来源。

2.3 其它TOPIC说明

2.3.1 CAN相关的TOPIC说明

• /canX/error_code: 接收来自上层控制节点的电机控制命令

• /canX/motor_cmd: 发布CAN总线上电机的实时状态反馈

• /canX/motor_states: 发布CAN总线上电机的错误代码和诊断信息

• /canX/set_zero: 用于设置指定电机的零点位置(发送一个整数值，表示要设置零点的电机编号)

2.3.2 MUX相关的TOPIC说明

/mux/chassis/cmd: 底盘控制命令的多路复用输出，该话题接收来自API模式的底盘速度命令，并通过mux节点转发到底盘控制节点，用于控制机器人底盘的移动。

• /mux/{$arm_side}_gripper/cmd: 左/右夹爪控制命令的多路复用输出，mux节点将来自API或VR手柄的夹爪控制命令转换为Joy消息格式，通过这些话题发布，用于控制左右夹爪的开合。

• /mux/{$arm_side}_target_ee_pose: 左/右臂末端执行器目标姿态的多路复用输出， 这些话题发布目标末端执行器（end effector）的姿态信息，用于逆运动学（IK）计算，支持VR模式和API模式的末端执行器控制。

• /mux/{$arm_side}_arm/mux_joint_cmd: 左/右臂关节命令的多路复用输出，mux节点将来自API模式的关节空间命令通过此话题发布，用于直接控制左/右臂的关节位置。

• /mux/xr/{$arm_side}_hand_inputs: VR手柄输入的多路复用输出，这些话题将VR手柄的原始输入数据通过mux节点转发，用于VR模式下的机器人控制。手柄输入包括按钮、轴、触发器等数据。

• /mux/servo/cmd: 伺服电机控制命令的多路复用输出，用于控制系统的伺服电机，可能包括使能/禁用状态或速度命令。

2.3.3 图像相关的TOPIC说明

相机分组

• left/ - 左腕部相机的所有数据流

• right/ - 右腕部相机的所有数据流

彩色图像相关话题

• /{$arm_side}/color/camera_info: 彩色相机的标定参数信息，包括内参矩阵、畸变系数、分辨率等

• /{$arm_side}/color/image_raw: 未压缩的原始彩色图像数据流

• /{$arm_side}/color/image_raw/ffmpeg: 使用FFmpeg编码压缩后的彩色视频流，用于降低带宽占用

• /{$arm_side}/color/metadata: 彩色图像的元数据，如时间戳、曝光参数、增益等

深度图像相关话题

• /{$arm_side}/depth/camera_info: 深度相机的标定参数信息

• /{$arm_side}/depth/image_raw: 未压缩的原始深度图像数据（每个像素表示距离值）

• /{$arm_side}/depth/image_raw/compressedDepth: 压缩后的深度图像，减少传输数据量

• /{$arm_side}/depth/metadata: 深度图像的元数据信息

对齐话题

• /{$arm_side}/depth_to_color: 将深度图像对齐到彩色图像坐标系的数据，实现RGB-D配准，使得深度信息和彩色信息的像素一一对应

主摄像头

• /xr_video_topic/ffmpeg: 主摄像头的图像数据

摄像头相关依赖的说明

对于需要使用高频camera sensor数据的场景，我们建议您使用ffmpeg格式的话题（/{$arm_side}/color/image_raw/ffmpeg，/xr_video_topic/ffmpeg）。相比较raw格式它对带宽占用等更加友好。在使用时您需要通过transport plugin republish解码后的图像topic，详细的使用请参见下方链接：

数据格式的转换（rosbag_to_lerobot）

对于有需要使用lerebot格式数据需求的用户，我们提供了转换工具，详细的使用请参见下方链接：

模型部署的说明

关于模型部署，我们提供了开箱即用的docker镜像，你可以参考下方链接的说明书完成模型部署的操作：

示例程序

3.1 末端模式demo

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
from std_msgs.msg import Float32
import numpy as np

def quaternion_slerp(q0, q1, t, shortest=True):
    q0 = np.array(q0, dtype=np.float64)
    q1 = np.array(q1, dtype=np.float64)

    q0 /= np.linalg.norm(q0)
    q1 /= np.linalg.norm(q1)

    dot = np.dot(q0, q1)
    if shortest and dot < 0.0:
        q1 = -q1
        dot = -dot
    dot = np.clip(dot, -1.0, 1.0)

    if np.abs(dot - 1.0) < 1e-6:
        return q0.tolist()

    theta = np.arccos(dot) * t
    sin_theta = np.sin(np.arccos(dot))
    q_rot = q0 * np.cos(theta) + (q1 - q0 * dot) * np.sin(theta) / sin_theta
    return q_rot.tolist()

class SinglePointMoveNode(Node):
    def __init__(self):
        super().__init__('single_point_move_node')

        # 发布器
        self.left_pose_pub = self.create_publisher(Pose, '/api/left_arm/target_pose', 10)
        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)

        # 订阅当前末端位姿
        self.left_pose_sub = self.create_subscription(
            Pose, '/left_arm/current_ee_pose', self.left_pose_callback, 10)

        self.enable_timer = self.create_timer(0.05, self.enable_callback)
        self.traj_timer = self.create_timer(0.02, self.timer_callback)

        # 目标位姿（用户定义）
        self.target_pose = Pose()
        self.target_pose.position.x = -0.014433992985935767
        self.target_pose.position.y = 0.0181230355198717
        self.target_pose.position.z = -0.5506818540802845
        self.target_pose.orientation.x = -0.651863775688076
        self.target_pose.orientation.y = 0.260180077341881
        self.target_pose.orientation.z = 0.7053938323707012
        self.target_pose.orientation.w = 0.0989923560353721

        self.current_left = None         
        self.traj_points = []             
        self.traj_index = 0               
        self.traj_active = False          
        self.plan_done = False            
        self.reached = False             

        self.pos_tolerance = 0.005        
        self.quat_dot_tolerance = 0.99    

    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)

    def left_pose_callback(self, msg):
        self.current_left = msg

        if not self.plan_done and self.current_left is not None:
            self.generate_trajectory()
            self.plan_done = True

        if not self.reached and self.current_left is not None:
            self.check_reached()

    def generate_trajectory(self):
        start = self.current_left
        p_start = [start.position.x, start.position.y, start.position.z]
        q_start = [start.orientation.x, start.orientation.y,
                   start.orientation.z, start.orientation.w]

        p_target = [self.target_pose.position.x, self.target_pose.position.y,
                    self.target_pose.position.z]
        q_target = [self.target_pose.orientation.x, self.target_pose.orientation.y,
                    self.target_pose.orientation.z, self.target_pose.orientation.w]

        # 根据直线距离决定插值步数（至少 20 步）
        dist = np.linalg.norm(np.array(p_target) - np.array(p_start))
        steps = max(int(dist / 0.01), 20)

        self.traj_points = []
        for i in range(steps + 1):
            t = i / steps

            px = p_start[0] + (p_target[0] - p_start[0]) * t
            py = p_start[1] + (p_target[1] - p_start[1]) * t
            pz = p_start[2] + (p_target[2] - p_start[2]) * t

            q_interp = quaternion_slerp(q_start, q_target, t)

            pose = Pose()
            pose.position.x = px
            pose.position.y = py
            pose.position.z = pz
            pose.orientation.x = q_interp[0]
            pose.orientation.y = q_interp[1]
            pose.orientation.z = q_interp[2]
            pose.orientation.w = q_interp[3]

            self.traj_points.append(pose)

        self.traj_index = 0
        self.traj_active = True
        self.get_logger().info(f"Trajectory generated with {len(self.traj_points)} points")

    def timer_callback(self):
        if not self.traj_active:
            return

        if self.traj_index < len(self.traj_points):
            self.left_pose_pub.publish(self.traj_points[self.traj_index])
            self.traj_index += 1
        else:
            self.traj_active = False
            self.get_logger().info("All trajectory points published, waiting for actual arrival...")

    def check_reached(self):
        """检查是否已到达目标位姿"""
        dx = self.current_left.position.x - self.target_pose.position.x
        dy = self.current_left.position.y - self.target_pose.position.y
        dz = self.current_left.position.z - self.target_pose.position.z
        pos_err = np.sqrt(dx*dx + dy*dy + dz*dz)

        q_curr = [self.current_left.orientation.x, self.current_left.orientation.y,
                  self.current_left.orientation.z, self.current_left.orientation.w]
        q_tar = [self.target_pose.orientation.x, self.target_pose.orientation.y,
                 self.target_pose.orientation.z, self.target_pose.orientation.w]

        q_curr = np.array(q_curr) / np.linalg.norm(q_curr)
        q_tar = np.array(q_tar) / np.linalg.norm(q_tar)
        dot = abs(np.dot(q_curr, q_tar))

        if pos_err < self.pos_tolerance and dot > self.quat_dot_tolerance:
            self.reached = True
            self.traj_active = False
            # 停止轨迹发布定时器
            self.traj_timer.cancel()
            # 停止 enable 定时器，并发送一次禁用信号
            if hasattr(self, 'enable_timer'):
                self.enable_timer.cancel()
            disable_msg = Float32()
            disable_msg.data = 0.0
            self.enable_pub.publish(disable_msg)
            self.left_pose_pub.publish(self.target_pose)
            self.get_logger().info(f"Target reached! Position error = {pos_err:.4f} m, "
                                   f"Quaternion dot = {dot:.4f}. Timer stopped.")

def main(args=None):
    rclpy.init(args=args)
    node = SinglePointMoveNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

3.2 夹爪控制demo

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from sensor_msgs.msg import JointState
from builtin_interfaces.msg import Time

class ApiDualGripperJointNode(Node):
    def __init__(self):
        super().__init__('api_dual_gripper_joint_test')
                
        
        # Gripper Publishers (Corrected topic names)
        self.left_gripper_pub = self.create_publisher(Float32, '/api/left_gripper/cmd', 10)
        self.right_gripper_pub = self.create_publisher(Float32, '/api/right_gripper/cmd', 10)

        self.left_joint_cmd_pub = self.create_publisher(JointState, '/api/left_arm/joint_cmd', 10)
        self.right_joint_cmd_pub = self.create_publisher(JointState, '/api/right_arm/joint_cmd', 10)       
        
        self.create_subscription(
            JointState,
            "/left_arm/joint_states",
            self.left_joint_state_callback,
            10
        )
        self.create_subscription(
            JointState,
            "/right_arm/joint_states",
            self.right_joint_state_callback,
            10
        )
        self.current_left_joint_state = None
        self.current_right_joint_state = None
        
        # Timers
        self.create_timer(0.02, self.timer_callback) # 50Hz
        
        self.get_logger().info("API Dual Gripper Joint Test Node Started")

        
    def left_joint_state_callback(self, msg):
        self.current_left_joint_state = msg

    def right_joint_state_callback(self, msg):
        self.current_right_joint_state = msg

    def timer_callback(self):
        current_time = self.get_clock().now()
        
        # Publish Gripper Command
        gripper_msg = Float32()
        # Target values: 0.04 (Open)
        gripper_msg.data = 0.04
        
        if self.current_left_joint_state is None or self.current_right_joint_state is None:
            self.get_logger().warn("Waiting for joint state data...")
            return
        
        left_joint_cmd = JointState()
        left_joint_cmd.header.stamp = current_time.to_msg()  
        left_joint_cmd.name = self.current_left_joint_state.name
        left_joint_cmd.position = self.current_left_joint_state.position
        left_joint_cmd.velocity = [0.0] * len(self.current_left_joint_state.name)  
        left_joint_cmd.effort = [0.0] * len(self.current_left_joint_state.name)
        
        right_joint_cmd = JointState()
        right_joint_cmd.header.stamp = current_time.to_msg()
        right_joint_cmd.name = self.current_right_joint_state.name
        right_joint_cmd.position = self.current_right_joint_state.position
        right_joint_cmd.velocity = [0.0] * len(self.current_right_joint_state.name)
        right_joint_cmd.effort = [0.0] * len(self.current_right_joint_state.name)
        
        self.left_joint_cmd_pub.publish(left_joint_cmd)
        self.right_joint_cmd_pub.publish(right_joint_cmd)
        
        self.left_gripper_pub.publish(gripper_msg)
        self.right_gripper_pub.publish(gripper_msg)

def main(args=None):
    rclpy.init(args=args)
    node = ApiDualGripperJointNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

3.3 关节模式demo

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float32
import numpy as np

class ApiDualArmJointNode(Node):
    def __init__(self):
        super().__init__('api_dual_arm_joint_test')
        
        # Publishers
        self.left_joint_cmd_pub = self.create_publisher(JointState, '/api/left_arm/joint_cmd', 10)
        self.right_joint_cmd_pub = self.create_publisher(JointState, '/api/right_arm/joint_cmd', 10)
        
        # Enable signal
        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)
        
        # Subscribers
        self.create_subscription(
            JointState,
            "/left_arm/joint_states",
            self.left_joint_state_callback,
            10
        )
        self.create_subscription(
            JointState,
            "/right_arm/joint_states",
            self.right_joint_state_callback,
            10
        )
        
        self.current_left_joint_state = None
        self.current_right_joint_state = None
        
        # Target values
        # Left: [0, 1.57, 0, 0.26, 0, 0, 0]
        # Right: [0, -1.57, 0, -0.26, 0, 0, 0] (Symmetric)
        self.target_left_position = np.array([0, 1.57, 0, 0.26, 0, 0, 0])
        self.target_right_position = np.array([0, -1.57, 0, -0.26, 0, 0, 0])
        
        # Timers
        self.create_timer(0.05, self.enable_callback)
        self.create_timer(0.02, self.timer_callback)
        
        self.get_logger().info("API Dual Arm Joint Test Node Started")

    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)

    def left_joint_state_callback(self, msg):
        self.current_left_joint_state = msg

    def right_joint_state_callback(self, msg):
        self.current_right_joint_state = msg

    def timer_callback(self):
        kp = 1.0
        clip_alpha = 0.25

        # Process Left Arm
        if self.current_left_joint_state is not None:
            self._process_arm(
                self.current_left_joint_state, 
                self.target_left_position, 
                self.left_joint_cmd_pub, 
                kp, clip_alpha, "Left"
            )
        else:
            self.get_logger().warning("Waiting for left joint states...", throttle_duration_sec=1.0)

        # Process Right Arm
        if self.current_right_joint_state is not None:
            self._process_arm(
                self.current_right_joint_state, 
                self.target_right_position, 
                self.right_joint_cmd_pub, 
                kp, clip_alpha, "Right"
            )
        else:
            self.get_logger().warning("Waiting for right joint states...", throttle_duration_sec=1.0)

    def _process_arm(self, current_state, target_pos, pub, kp, clip_alpha, arm_name):
        current_pos = np.array(current_state.position)
        
        if len(current_pos) != len(target_pos):
            self.get_logger().warning(f"{arm_name} dimension mismatch: {len(current_pos)} vs {len(target_pos)}", throttle_duration_sec=1.0)
            return
        
        velocity = np.clip(
            kp * (target_pos - current_pos),
            -clip_alpha, clip_alpha
        ).tolist()
        
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = current_state.name
        msg.position = target_pos.tolist()
        msg.velocity = velocity
        msg.effort = [0.0] * len(target_pos)
        
        pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = ApiDualArmJointNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

3.4 底盘控制demo

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray,Float32

class ChassisVelocityPublisher(Node):
    """
    底盘速度命令发布节点（持续发布一小段时间后自动退出）
    发布到 /api/chassis/velocity 话题（请确认实际订阅话题）
    """

    def __init__(self):
        super().__init__('chassis_velocity_api_publisher')

        # 创建发布者（如果实际订阅话题是 /mux/chassis/cmd，请修改话题名）
        self.chassis_vel_pub = self.create_publisher(
            Float32MultiArray,
            '/api/chassis/velocity',   # 根据实际情况修改
            10
        )

        # 发布频率 10Hz
        self.publish_rate = 10.0
        self.timer = self.create_timer(1.0 / self.publish_rate, self.publish_velocity_cmd)

        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)
        self.create_timer(0.05, self.enable_callback)

        # 速度命令（初始为0）
        self.linear_x = 0.0
        self.linear_y = 0.0
        self.angular_z = 0.0

        # 2秒后自动停止的定时器
        self.shutdown_timer = self.create_timer(2.0, self.shutdown_callback)

        self.get_logger().info('Chassis velocity publisher started (will run for 2 seconds)')


    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)


    def publish_velocity_cmd(self):
        """定时发布速度命令"""
        msg = Float32MultiArray()
        msg.data = [self.linear_x, self.linear_y, self.angular_z]
        self.chassis_vel_pub.publish(msg)
        self.get_logger().info(
            f'Published: linear_x={self.linear_x:.2f}, '
            f'linear_y={self.linear_y:.2f}, angular_z={self.angular_z:.2f}'
        )

    def shutdown_callback(self):
        """2秒后停止：发布零速度并关闭节点"""
        self.get_logger().info('Test finished, stopping robot.')

        # 发布零速度确保停止
        stop_msg = Float32MultiArray()
        stop_msg.data = [0.0, 0.0, 0.0]
        self.chassis_vel_pub.publish(stop_msg)

        # 取消所有定时器
        self.timer.cancel()
        self.shutdown_timer.cancel()

        # 设置标志，让主循环退出
        self.shutdown_flag = True

    def set_velocity(self, linear_x, linear_y, angular_z):
        """设置速度值"""
        self.linear_x = linear_x
        self.linear_y = linear_y
        self.angular_z = angular_z


def main(args=None):
    rclpy.init(args=args)

    # 创建节点
    chassis_publisher = ChassisVelocityPublisher()

    # 设置微小直线速度（前进0.05 m/s，不转弯）
    chassis_publisher.set_velocity(
        linear_x=0.05,   # 可调整，建议0.02~0.1
        linear_y=0.0,
        angular_z=0.0
    )

    # 自定义主循环，检测退出标志
    try:
        while rclpy.ok() and not hasattr(chassis_publisher, 'shutdown_flag'):
            rclpy.spin_once(chassis_publisher, timeout_sec=0.1)
    except KeyboardInterrupt:
        chassis_publisher.get_logger().info('Interrupted by user')
    finally:
        # 确保最后发布零速度
        stop_msg = Float32MultiArray()
        stop_msg.data = [0.0, 0.0, 0.0]
        chassis_publisher.chassis_vel_pub.publish(stop_msg)

        chassis_publisher.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

初始位置demo（基于关节模式）

robot_name: TeleAvatar
arms:
  dof_num: 7
  max_arm_len: 0.55
  right_arm:
    upper: [ 1.8, 0.0, 2.6, -0.25, 1.8, 1.4, 0.6 ]
    lower: [ -1.8, -1.9, -1.3, -1.9, -1.8, -1.4, -0.6 ]
  left_arm:
    upper: [ 1.8, 1.9, 1.3, 1.90, 1.8, 1.4, 0.6 ]
    lower: [ -1.8, 0.0, -2.6, 0.25, -1.8, -1.4, -0.6 ]
  vel_limit: [ 15.0, 15.0, 20.0, 20.0, 44.0, 33.0, 33.0 ]
  acc_limit: [ 120.0, 120.0, 60.0, 60.0, 17.0, 14.0, 14.0 ]

ik_solver:
  debug: false
  arm_ctrl_dt: 0.01
  ee_predict_alpha: 1.0
  base_frame: base
  ik_pub_topic: ik_joint_cmd
  states_sub_topic: joint_states
  right_arm:
    ee_sub_topic_name: right_target_ee_pose
    shoulder_frame_name: right_shoulder_base
    target_ee_frame_name: right_target_ee_frame
    curret_ee_frame_name: right_current_ee_frame
    joint_name_prefix: r_joint
    T_lee2pee: [ [ 0, -1, 0, 0 ], [ 1, 0, 0, 0 ], [ 0, 0, 1, 0 ], [ 0, 0, 0, 1 ] ]
  left_arm:
    ee_sub_topic_name: left_target_ee_pose
    shoulder_frame_name: left_shoulder_base
    target_ee_frame_name: left_target_ee_frame
    curret_ee_frame_name: left_current_ee_frame
    joint_name_prefix: l_joint
    T_lee2pee: [ [ 0, 1, 0, 0 ], [ -1, 0, 0, 0 ], [ 0, 0, 1, 0 ], [ 0, 0, 0, 1 ] ]

  vel:
    fb_kp: [ 7, 7, 10, 10, 10, 8, 8 ]
    ff_alpha: 0.3

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from rclpy.clock import Clock
from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import Float32

import numpy as np
import yaml

robot_config = yaml.safe_load(open("arm_config.yml"))

class JointInterpolator(Node):
    def __init__(self, namespace="right_arm", down=True, timer_on=False):
        super().__init__(f"{namespace}_joint_interpolator")
        self.namespace = namespace
        self.down = down

        self.subscription = self.create_subscription(
            JointState,
            f"/{namespace}/joint_states",
            self.joint_state_callback,
            10,
        )
        self.publisher = self.create_publisher(
            JointState, f"/api/{namespace}/joint_cmd", 10
        )
        
        # Enable signal
        if timer_on == True:
            self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)
            self.create_timer(0.05, self.enable_callback)

        self.err_publisher = self.create_publisher(
            JointState, f"/{namespace}/traj_error", 10
        )
        self.current_pos = None
        self.start_time = None

        self.joint_names = []
        self.duration = 5.0  # seconds
        self.target_positions = []

        
        self.position_tolerance = 0.05  
        self.convergence_duration = 0.5  
        self.convergence_start_time = None
        self.is_converged = False

        num_activate = 7
        self.enable_flags = [False] * (7 - num_activate) + [True] * num_activate

        self.upper = np.array(robot_config["arms"][namespace]["upper"])
        self.lower = np.array(robot_config["arms"][namespace]["lower"])
        # Timer to run at 100 Hz for publishing commands
        self.timer = self.create_timer(0.01, self.timer_callback)  # 100 Hz

        if self.down:
            joint_2_down_pos = 1.5 if self.namespace == "left" else -1.5
            joint_4_down_pos = 0.0
        else:
            joint_2_down_pos, joint_4_down_pos = 0, 0
        
        # 根据namespace设置不同的目标位置
        if self.namespace == "right_arm":
            # 右臂目标位置
            self.target_positions = [
                -0.50,  # r_joint1
                -0.92,  # r_joint2
                0.52,  # r_joint3
                -1.28,    # r_joint4
                0.32,  # r_joint5
                0.55, # r_joint6
                -0.52   # r_joint7
            ]
        else:  # left
            # 左臂目标位置
            self.target_positions = [
                0.41,  # l_joint1
                1.16,  # l_joint2
                -0.47,  # l_joint3
                0.90,    # l_joint4
                0.23,  # l_joint5
                -0.15, # l_joint6
                0.60   # l_joint7
            ]

        self.get_logger().info(
            f"Joint interpolator node initialized for {namespace}. Waiting for initial joint states..."
        )
        self.get_logger().info(f"Position tolerance: {self.position_tolerance} rad")
        self.get_logger().info(f"Convergence duration: {self.convergence_duration} sec")

    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)


    def joint_state_callback(self, msg):
        self.joint_names = msg.name
        self.current_pos = np.array(msg.position)
        self.current_vel = np.array(msg.velocity)

    def check_convergence(self, current_position, target_position):
        
        enabled_indices = [i for i, enabled in enumerate(self.enable_flags) if enabled]
        position_errors = np.abs(np.array(current_position) - np.array(target_position))

       
        enabled_errors = position_errors[enabled_indices]
        max_error = np.max(enabled_errors)

        return max_error < self.position_tolerance

    def timer_callback(self):
        
        if self.current_pos is None or self.start_time is None:
            self.get_logger().warn(
                "Current position not set yet, skipping timer callback."
            )
            self.start_time = self.get_clock().now()
            return

        current_time = self.get_clock().now()
        elapsed_time = (
            current_time - self.start_time
        ).nanoseconds / 1e9  # Convert to seconds

        cmd_msg = JointState()
        cmd_msg.header.stamp = current_time.to_msg()
        cmd_msg.name = self.joint_names

        if elapsed_time >= self.duration:
            # Publish target positions (all zeros) once duration is reached
            cmd_msg.position = self.target_positions
        else:
            # Linear interpolation between initial and target positions
            alpha = elapsed_time / self.duration
            interpolated_positions = [
                self.current_pos[i] * (1.0 - alpha) + self.target_positions[i] * alpha
                for i in range(len(self.current_pos))
            ]
            cmd_msg.position = interpolated_positions

        cmd_msg.velocity = [0.0] * 7
        des_position = np.array(cmd_msg.position)
        kp = np.array([ 7, 7, 10, 10, 10, 8, 8 ])
        vel_fb = kp * (des_position - self.current_pos)

        for i in range(len(self.enable_flags)):
            if self.enable_flags[i]:
                cmd_msg.position[i] = self.current_pos[i]
                cmd_msg.velocity[i] = vel_fb[i]
        cmd_msg.effort = [0.0] * 7
        self.publisher.publish(cmd_msg)

        
        msg = JointState()
        msg.header.stamp = current_time.to_msg()
        msg.name = self.joint_names
        msg.position = [
            self.current_pos[i] - cmd_msg.position[i]
            for i in range(len(self.current_pos))
        ]
        self.err_publisher.publish(msg)

        
        if elapsed_time >= self.duration: 
            target_pos = np.array(self.target_positions)
            if self.check_convergence(self.current_pos, target_pos):
                if self.convergence_start_time is None:
                    self.convergence_start_time = current_time
                    self.get_logger().info(
                        f"{self.namespace} arm: Position converged, starting convergence timer..."
                    )
                else:
                    convergence_elapsed = (
                        current_time - self.convergence_start_time
                    ).nanoseconds / 1e9
                    if convergence_elapsed >= self.convergence_duration:
                        if not self.is_converged:
                            self.get_logger().info(
                                f"{self.namespace} arm: Convergence achieved! Stopping control."
                            )
                            self.is_converged = True
                            
            else:
                
                if self.convergence_start_time is not None:
                    self.get_logger().info(
                        f"{self.namespace} arm: Position diverged, resetting convergence timer..."
                    )
                    self.convergence_start_time = None

def main(args=None):
    rclpy.init(args=args)
    joint_interpolator = JointInterpolator()
    rclpy.spin(joint_interpolator)
    joint_interpolator.destroy_node()
    rclpy.shutdown()

def main_multithreaded(args=None):
    rclpy.init(args=args)

    left_node = JointInterpolator(namespace="left_arm", down=True, timer_on=True)  # left arm
    right_node = JointInterpolator(namespace="right_arm", down=True, timer_on=True)  # right arm

    executor = MultiThreadedExecutor()
    executor.add_node(left_node)
    executor.add_node(right_node)

    try:
        print("Starting both arm nodes with MultiThreadedExecutor...")
        while rclpy.ok():
            executor.spin_once(timeout_sec=0.1)

            
            if left_node.is_converged and right_node.is_converged:
                print("Both arms converged! Shutting down...")
                break

    except KeyboardInterrupt:
        print("Shutting down...")
    finally:
        left_node.destroy_node()
        right_node.destroy_node()
        executor.shutdown()
        rclpy.shutdown()

if __name__ == "__main__":
    main_multithreaded()
    

6.6 升降电机demo（仅适用于升降版）

#!/usr/bin/env python3
  import time

  import rclpy
  from rclpy.node import Node
  from std_msgs.msg import Float32MultiArray


  class LiftServoApiDemo(Node):
      def __init__(self):
          super().__init__("lift_servo_api_demo")
          self.servo_command_pub = self.create_publisher(
              Float32MultiArray,
              "/api/servo/cmd",
              10,
          )

      def publish_servo_command(self, normalized_velocity: float, servo_enabled: bool):
          normalized_velocity = max(-1.0, min(1.0, float(normalized_velocity)))

          if not servo_enabled:
              normalized_velocity = 0.0

          msg = Float32MultiArray()
          msg.data = [
              normalized_velocity,           # 归一化速度指令 [-1, 1]
              1.0 if servo_enabled else 0.0, # 使能标志
          ]
          self.servo_command_pub.publish(msg)

      def publish_for_duration(
          self,
          normalized_velocity: float,
          servo_enabled: bool,
          duration_sec: float,
          rate_hz: float = 20.0,
      ):
          dt = 1.0 / rate_hz
          end_time = time.time() + duration_sec

          while rclpy.ok() and time.time() < end_time:
              self.publish_servo_command(normalized_velocity, servo_enabled)
              rclpy.spin_once(self, timeout_sec=0.0)
              time.sleep(dt)

      def stop_and_disable(self):
          self.publish_servo_command(0.0, False)

      def run_demo(self):
          self.get_logger().info("Lift up")
          self.publish_for_duration(
              normalized_velocity=0.30,
              servo_enabled=True,
              duration_sec=1.5,
          )

          self.get_logger().info("Pause")
          self.publish_for_duration(
              normalized_velocity=0.0,
              servo_enabled=True,
              duration_sec=0.4,
          )

          self.get_logger().info("Lift down")
          self.publish_for_duration(
              normalized_velocity=-0.30,
              servo_enabled=True,
              duration_sec=1.5,
          )

          self.get_logger().info("Stop")
          self.stop_and_disable()


  def main(args=None):
      rclpy.init(args=args)
      node = LiftServoApiDemo()

      try:
          node.run_demo()
      except KeyboardInterrupt:
          pass
      finally:
          node.stop_and_disable()


  if __name__ == "__main__":
      main()

结构图

关节限位信息

支持