接口使用文档

SDK 概述

该 SDK 提供了一系列用于控制和获取机器人状态的接口，主要分为两类：话题（Topic）和服务（Service）。这些接口基于 ROS（机器人操作系统）框架，提供 Python 示例进行开发和调用。以下是 SDK 的主要功能概述：

话题（Topic）

话题用于发布和订阅机器人传感器数据和控制指令。通过话题，用户可以实时获取机器人传感器的原始数据，如 IMU 数据、点云数据、图像数据等，也可以发布控制指令来控制机器人的运动和姿态。

• 传感器数据：通过话题获取机器人传感器的原始数据，包括 IMU 数据、点云数据、图像数据等。
• 运动控制：通过话题发布控制指令，指定机器人的运动轨迹、速度和姿态。

服务（Service）

服务用于执行特定的控制命令或获取状态信息。服务调用是同步的，即用户发送请求后会等待服务返回响应。服务通常用于执行一次性操作，如播放音乐、录制音频、设置控制模式等。

• 控制命令：通过服务接口发送控制命令，如播放音乐、录制音频、设置手臂控制模式等。
• 状态获取：通过服务接口获取机器人的状态信息。

使用指南

1. 初始化 ROS 节点：在使用 SDK 之前，需要初始化一个 ROS 节点。可以使用 rospy.init_node() 方法来完成节点的初始化。

2. 创建话题订阅者或发布者：根据需要创建话题的订阅者或发布者。订阅者用于接收话题数据，发布者用于发送控制指令。

3. 创建服务代理：对于需要调用的服务，创建一个服务代理对象，并通过该对象发送请求和接收响应。

4. 处理数据或响应：在接收到话题数据或服务响应后，进行相应的处理或操作。

• 接口使用文档
  • SDK 概述
    • 话题（Topic）
    • 服务（Service）
  • 使用指南
  • /play_music 播放音频
  • /record_music 记录音频
  • /livox/imu 雷达imu数据
  • /livox/lidar 雷达点云数据
  • /camera/depth/color/points 相机点云数据
  • /camera/color/image_raw 相机彩色图像数据
  • /camera/depth/image_rect_raw 相机深度图像数据
  • /arm_traj_change_mode 设置手臂控制模式
  • /kuavo_arm_target_poses 手臂运动控制(指定时间内到达目标位置)
  • /kuavo_arm_traj 手臂运动控制(用于自定义手臂运动轨迹规划)
  • /robot_head_motion_data 头部关节控制
  • /control_robot_hand_position 灵巧手控制
  • /cmd_pose 位置控制
  • /cmd_vel 速度控制
  • /humanoid_controller/real_initial_start 执行机器人站立
  • /sensors_data_raw 传感器数据
  • /ik/fk_srv 机器人手臂FK正解
  • /ik/two_arm_hand_pose_cmd_srv 机器人手臂IK逆解
  • /control_robot_leju_claw 控制机器人夹爪（二指爪）运动
  • /leju_claw_state 获取机器人夹爪（二指爪）状态
  • /leju_quest_bone_poses VR骨骼位姿数据

/play_music 播放音频

1. 功能描述

/play_music 服务用于控制机器人播放指定的音乐文件。用户可以通过提供音乐文件的名称或编号以及音量来实现音乐播放。

2. 请求格式

• 类型: playmusicRequest
• 字段:
  • music_number (str): 音乐文件的名称或编号。可以是文件路径或预定义的音乐编号。
  • volume (int): 音乐的音量，范围通常为 0 到 100。

3. 响应格式

• 类型: playmusicResponse
• 字段:
  • success_flag (bool): 表示音乐播放请求是否成功。True 表示成功，False 表示失败。

4. 使用示例

import rospy
from kuavo_msgs.srv import playmusic, playmusicRequest

# 初始化 ROS 节点
rospy.init_node('music_player_client')

# 创建服务代理
robot_music_play_client = rospy.ServiceProxy("/play_music", playmusic)

# 创建请求对象
request = playmusicRequest()
request.music_number = "/home/kuavo/你好"  # 音乐文件路径
request.volume = 80  # 音量

# 发送请求并接收响应
response = robot_music_play_client(request)

---

/record_music 记录音频

1. 功能描述

/record_music 服务用于控制机器人录制指定的音乐文件。用户可以通过提供音乐文件的编号以及超时时间来实现音乐录制。

2. 请求格式

• 类型: recordmusicRequest
• 字段:
  • music_number (str): 音乐文件的编号。用于标识要录制的音乐文件。
  • time_out (int): 超时时间，以秒为单位。指定录制操作的最大持续时间。

3. 响应格式

• 类型: recordmusicResponse
• 字段:
  • success_flag (bool): 表示音乐录制请求是否成功。True 表示成功，False 表示失败。

4. 使用示例

import rospy
from kuavo_msgs.srv import recordmusic, recordmusicRequest

# 初始化 ROS 节点
rospy.init_node('music_recorder')

# 创建服务代理
robot_record_music_client = rospy.ServiceProxy("/record_music", recordmusic)

# 创建请求对象
request = recordmusicRequest()
request.music_number = "example_music"  # 音乐文件编号
request.time_out = 30  # 超时时间

# 调用服务并获取响应
response = robot_record_music_client(request)

---

/livox/imu 雷达imu数据

参考雷达启动启动雷达节点

1. 功能描述

/livox/imu 话题用于提供雷达内置 IMU（惯性测量单元）信息，包括线性加速度和角速度。

2. 消息类型

• 类型: sensor_msgs/Imu

3. 消息字段

• Header: 消息头信息

  • seq (uint32): 序列号
  • stamp (time): 时间戳
  • frame_id (string): 坐标系 ID

• orientation: 方向四元数

  • x, y, z, w (float64): 四元数的分量

• orientation_covariance: 方向协方差矩阵 (float64[9])

• angular_velocity: 角速度

  • x, y, z (float64): 角速度的分量

• angular_velocity_covariance: 角速度协方差矩阵 (float64[9])

• linear_acceleration: 线性加速度

  • x, y, z (float64): 加速度的分量

• linear_acceleration_covariance: 加速度协方差矩阵 (float64[9])

4. 使用示例

import rospy
from sensor_msgs.msg import Imu

def imu_callback(data):
    rospy.loginfo("Received IMU data: %s", data)

rospy.init_node('imu_listener')
rospy.Subscriber("/livox/imu", Imu, imu_callback)
rospy.spin()

---

/livox/lidar 雷达点云数据

参考雷达启动启动雷达节点

1. 功能描述

/livox/lidar 话题用于提供雷达的点云数据信息，用于三维空间的点云表示。

2. 消息类型

• 类型: sensor_msgs/PointCloud2

3. 消息字段

• Header: 消息头信息

  • seq (uint32): 序列号
  • stamp (time): 时间戳
  • frame_id (string): 坐标系 ID

• height (uint32): 点云的高度（通常为 1）

• width (uint32): 点云的宽度（点的数量）

• fields: 点字段信息

  • name (string): 字段名称
  • offset (uint32): 字段偏移
  • datatype (uint8): 数据类型
  • count (uint32): 字段计数

• is_bigendian (bool): 是否为大端序

• point_step (uint32): 每个点的字节数

• row_step (uint32): 每行的字节数

• data (uint8[]): 点云数据

• is_dense (bool): 是否为密集点云

4. 使用示例

import rospy
from sensor_msgs.msg import PointCloud2

def lidar_callback(data):
    rospy.loginfo("Received LIDAR data: %s", data)

rospy.init_node('lidar_listener')
rospy.Subscriber("/livox/lidar", PointCloud2, lidar_callback)
rospy.spin()

---

/camera/depth/color/points 相机点云数据

1. 功能描述

/camera/depth/color/points 话题用于提供相机的点云数据，结合了深度信息和颜色信息，用于三维空间的点云表示。

2. 消息类型

• 类型: sensor_msgs/PointCloud2

3. 消息字段

• Header: 消息头信息

  • seq (uint32): 序列号
  • stamp (time): 时间戳
  • frame_id (string): 坐标系 ID

• height (uint32): 点云的高度（通常为 1）

• width (uint32): 点云的宽度（点的数量）

• fields: 点字段信息

  • name (string): 字段名称
  • offset (uint32): 字段偏移
  • datatype (uint8): 数据类型
  • count (uint32): 字段计数

• is_bigendian (bool): 是否为大端序

• point_step (uint32): 每个点的字节数

• row_step (uint32): 每行的字节数

• data (uint8[]): 点云数据

• is_dense (bool): 是否为密集点云

4. 使用示例

import rospy
from sensor_msgs.msg import PointCloud2

def points_callback(data):
    rospy.loginfo("Received point cloud data")

rospy.init_node('points_listener')
rospy.Subscriber("/camera/depth/color/points", PointCloud2, points_callback)
rospy.spin()

---

/camera/color/image_raw 相机彩色图像数据

1. 功能描述

/camera/color/image_raw 话题用于提供相机的原始彩色图像数据。

2. 消息类型

• 类型: sensor_msgs/Image

3. 消息字段

• Header: 消息头信息

  • seq (uint32): 序列号
  • stamp (time): 时间戳
  • frame_id (string): 坐标系 ID

• height (uint32): 图像高度

• width (uint32): 图像宽度

• encoding (string): 图像编码格式（如 rgb8）

• is_bigendian (uint8): 是否为大端序

• step (uint32): 每行的字节数

• data (uint8[]): 图像数据

4. 使用示例

import rospy
from sensor_msgs.msg import Image

def image_callback(data):
    rospy.loginfo("Received raw image data")

rospy.init_node('image_listener')
rospy.Subscriber("/camera/color/image_raw", Image, image_callback)
rospy.spin()

---

/camera/depth/image_rect_raw 相机深度图像数据

1. 功能描述

/camera/depth/image_rect_raw 话题用于提供相机的深度图像数据，经过校正以消除畸变。

2. 消息类型

• 类型: sensor_msgs/Image

3. 消息字段

• Header: 消息头信息

  • seq (uint32): 序列号
  • stamp (time): 时间戳
  • frame_id (string): 坐标系 ID

• height (uint32): 图像高度

• width (uint32): 图像宽度

• encoding (string): 图像编码格式（如 16UC1）

• is_bigendian (uint8): 是否为大端序

• step (uint32): 每行的字节数

• data (uint8[]): 图像数据

4. 使用示例

import rospy
from sensor_msgs.msg import Image

def depth_image_callback(data):
    rospy.loginfo("Received depth image data")

rospy.init_node('depth_image_listener')
rospy.Subscriber("/camera/depth/image_rect_raw", Image, depth_image_callback)
rospy.spin()

---

/arm_traj_change_mode 设置手臂控制模式

1. 功能描述

/arm_traj_change_mode 服务用于设置 OCS2 手臂的控制模式。用户可以通过提供控制模式编号来更改手臂的操作方式。

控制模式包括：

• 0: 保持姿势（keep pose）
• 1: 行走时自动摆手（auto_swing_arm）
• 2: 外部控制（external_control）

2. 请求格式

• 类型: changeArmCtrlModeRequest

• 字段:

  • control_mode (int): 要设置的控制模式编号。有效值为 0、1 或 2。

3. 响应格式

• 类型: changeArmCtrlModeResponse

• 字段:

  • result (bool): 表示控制模式更改请求是否成功。True 表示成功，False 表示失败。
  • message (str): 包含关于操作结果的详细信息的消息。

4. 使用示例

import rospy
from kuavo_msgs.srv import changeArmCtrlMode, changeArmCtrlModeRequest, changeArmCtrlModeResponse

# 初始化 ROS 节点
rospy.init_node('arm_control_mode_client')

# 创建服务代理
arm_traj_change_mode_client = rospy.ServiceProxy("/arm_traj_change_mode", changeArmCtrlMode)

# 创建请求对象
request = changeArmCtrlModeRequest()
request.control_mode = 2  # 设置控制模式

# 调用服务并获取响应
response = arm_traj_change_mode_client(request)

---

/kuavo_arm_target_poses 手臂运动控制(指定时间内到达目标位置)

1. 功能描述

/kuavo_arm_target_poses 话题用于发布手臂的目标姿态信息，包括时间和关节角度。该话题可以用于控制手臂运动到指定的姿态。

2. 消息类型

• 类型: kuavo_msgs/armTargetPoses

3. 消息字段

• times (list of float): 时间列表，表示每个姿态的目标时间点。
• values (list of float): 关节角度列表，表示手臂在每个时间点的目标关节角度。

4. 使用示例

import rospy
from kuavo_msgs.msg import armTargetPoses

# 初始化ROS节点
rospy.init_node('arm_target_poses_publisher')

# 创建发布者
pub = rospy.Publisher('kuavo_arm_target_poses', armTargetPoses, queue_size=10)

# 创建消息对象
msg = armTargetPoses()
msg.times = [3]  # 时间列表
msg.values = [-20, 0, 0, -30, 0, 0, 0, 20, 0, 0, -30, 0, 0, 0]  # 关节角度列表

# 发布消息
pub.publish(msg)

---

/kuavo_arm_traj 手臂运动控制(用于自定义手臂运动轨迹规划)

1. 功能描述

/kuavo_arm_traj 话题用于控制机器人手臂运动，通过发布手臂目标关节位置来实现手臂的精确控制

2. 消息类型

• 类型: sensor_msgs/JointState

3. 消息字段

• name (list of string): 关节名称列表，假设有 14 个关节，名称为 "arm_joint_1" 到 "arm_joint_14"。
• position (list of float): 当前关节位置列表。
• header.stamp (time): 消息的时间戳，设置为当前时间。

4. 使用示例

import rospy
from sensor_msgs.msg import JointState
import numpy as np

# 初始化ROS节点
rospy.init_node('sim_traj')

# 创建发布者
pub = rospy.Publisher("/kuavo_arm_traj", JointState, queue_size=10)

# 等待直到有订阅者连接
while pub.get_num_connections() == 0:
    rospy.sleep(0.1)  # 适当的睡眠时间，避免 CPU 占用过高

msg = JointState()
msg.name = ["arm_joint_" + str(i) for i in range(1, 15)]  # 关节名称列表
msg.header.stamp = rospy.Time.now()  # 当前时间戳
msg.position = np.array([-30, 60, 0, -30, 0, -30, 30, 0, 0, 0, 0, 0, 0, 0])  # 关节位置列表

# 发布消息
pub.publish(msg)

---

/robot_head_motion_data 头部关节控制

1. 功能描述

/robot_head_motion_data 话题用于发布机器人头部的目标运动数据，包括偏航角和俯仰角。该话题可以用于控制机器人的头部运动。

2. 消息类型

• 类型: kuavo_msgs/robotHeadMotionData

3. 消息字段

• joint_data (list of float): 包含头部偏航角和俯仰角的列表。偏航角范围为 [-30, 30] 度，俯仰角范围为 [-25, 25] 度。

4. 使用示例

import rospy
from kuavo_msgs.msg import robotHeadMotionData

# 初始化ROS节点
rospy.init_node('robot_head_controller')

# 创建发布者
pub_head_pose = rospy.Publisher('/robot_head_motion_data', robotHeadMotionData, queue_size=10)

# 创建消息对象
head_target_msg = robotHeadMotionData()
head_target_msg.joint_data = [0, 0]  # 偏航角和俯仰角

# 发布消息
pub_head_pose.publish(head_target_msg)

---

/control_robot_hand_position 灵巧手控制

1. 功能描述

/control_robot_hand_position 话题用于控制机器人双手(手指)的运动，通过发布手指目标关节位置来实现手部的精确控制。

2. 话题类型

• 类型: kuavo_msgs/robotHandPosition

3. 消息字段

• left_hand_position (list of float): 左手位置，包含6个元素，每个元素的取值范围为[0, 100], 0 为张开，100 为闭合。
• right_hand_position (list of float): 右手位置，包含6个元素，每个元素的取值范围为[0, 100], 0 为张开，100 为闭合。

4. 使用示例

import rospy
from kuavo_msgs.msg import robotHandPosition

# 初始化ROS节点
rospy.init_node('robot_hand_controller')

# 初始化话题发布者
pub = rospy.Publisher('/control_robot_hand_position', robotHandPosition, queue_size=10)

# 创建消息对象
msg = robotHandPosition()
msg.left_hand_position = [0, 0, 0, 0, 0, 0]  # 左手位置
msg.right_hand_position = [20, 20, 20, 20, 20, 20]  # 右手位置

# 发布消息
pub.publish(msg)

---

/cmd_pose 位置控制

1. 功能描述

/cmd_pose 话题用于发布控制指令，指定机器人在空间中的线速度和角速度。

2. 消息类型

• 类型: geometry_msgs/Twist

3. 消息字段

• linear.x (float): 基于当前位置的 x 方向值，单位为米 (m)。
• linear.y (float): 基于当前位置的 y 方向值，单位为米 (m)。
• linear.z (float): 增量高度 (m)。
• angular.x (float): 未使用，设置为 0。
• angular.y (float): 未使用，设置为 0。
• angular.z (float): 基于当前位置旋转（偏航）的角度，单位为弧度 (radian)。

4. 使用示例

import rospy
from geometry_msgs.msg import Twist

# 初始化ROS节点
rospy.init_node('cmd_pose_publisher')

# 创建发布者
cmd_pose_pub = rospy.Publisher('/cmd_pose', Twist, queue_size=10)

# 创建Twist消息对象
cmd_pose_msg = Twist()
cmd_pose_msg.linear.x = 0.5  # 基于当前位置的 x 方向值 (m)
cmd_pose_msg.linear.y = 0.0  # 基于当前位置的 y 方向值 (m)
cmd_pose_msg.linear.z = 0.0  # 增量高度
cmd_pose_msg.angular.z = 0.0  # # 基于当前位置旋转（偏航）的角度，单位为弧度 (radian)

# 发布消息
cmd_pose_pub.publish(cmd_pose_msg)

---

/cmd_vel 速度控制

1. 功能描述

/cmd_vel 话题用于发布控制指令，指定机器人在空间中的线速度和角速度

2. 消息类型

• 类型: geometry_msgs/Twist

3. 消息字段

• linear.x (float): x 方向线速度 (m/s)。
• linear.y (float): y 方向线速度 (m/s)。
• linear.z (float): 增量高度 (m)。
• angular.x (float): 未使用，设置为 0。
• angular.y (float): 未使用，设置为 0。
• angular.z (float): yaw 方向角速度 (radian/s)。

4. 使用示例

import rospy
from geometry_msgs.msg import Twist

# 初始化ROS节点
rospy.init_node('cmd_vel_publisher')

# 创建发布者
cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

# 设置发布频率
rate = rospy.Rate(10)  # 10 Hz

# 创建Twist消息对象
cmd_vel_msg = Twist()
cmd_vel_msg.linear.x = 0.2  # x 方向速度
cmd_vel_msg.linear.y = 0.0  # y 方向速度
cmd_vel_msg.linear.z = 0.0  # 增量高度
cmd_vel_msg.angular.z = 0.0  # yaw 方向角速度

while not rospy.is_shutdown():
    # 发布消息
    cmd_vel_pub.publish(cmd_vel_msg)
    # 等待下一个发布周期
    rate.sleep()

---

/humanoid_controller/real_initial_start 执行机器人站立

1. 功能描述

/humanoid_controller/real_initial_start 服务用于触发机器人的初始化过程：机器人cali状态执行一次机器人缩腿，再执行一次机器人站立，用户需要在站立过程中用手扶住机器人以确保安全，实机才有该服务

2. 服务类型

• 类型: std_srvs/Trigger

3. 请求消息

• 无需请求参数。

4. 响应消息

• success (bool): 服务调用结果，成功返回 True，失败返回 False。
• message (string): 服务调用的响应消息，提供成功或失败的详细信息。

5. 使用示例

import rospy
from std_srvs.srv import Trigger

# 初始化ROS节点
rospy.init_node('init_trigger_service_caller')

# 创建服务代理
trigger_init_service = rospy.ServiceProxy('/humanoid_controller/real_initial_start', Trigger)

# 调用服务并获取响应
response = trigger_init_service()

---

/sensors_data_raw 传感器数据

1. 功能描述

/sensors_data_raw 话题用于发布实物机器人或仿真器的传感器原始数据，包括关节数据、IMU数据和末端执行器数据。

2. 消息类型

• 类型: kuavo_msgs/sensorsData

3. 消息字段

字段	类型	描述
sensor_time	time	时间戳
joint_data	kuavo_msgs/jointData	关节数据: 位置、速度、加速度、电流
imu_data	kuavo_msgs/imuData	包含陀螺仪、加速度计、自由加速度、四元数
end_effector_data	kuavo_msgs/endEffectorData	末端数据，暂未使用

4. 关节数据说明

• 数组长度: NUM_JOINT

• 数据顺序:

  • 前 12 个数据为下肢电机数据:
    • 0~5 为左下肢数据 (l_leg_roll, l_leg_yaw, l_leg_pitch, l_knee, l_foot_pitch, l_foot_roll)
    • 6~11 为右下肢数据 (r_leg_roll, r_leg_yaw, r_leg_pitch, r_knee, r_foot_pitch, r_foot_roll)
  • 接着 14 个数据为手臂电机数据:
    • 12~18 左臂电机数据 ("l_arm_pitch", "l_arm_roll", "l_arm_yaw", "l_forearm_pitch", "l_hand_yaw", "l_hand_pitch", "l_hand_roll")
    • 19~25 为右臂电机数据 ("r_arm_pitch", "r_arm_roll", "r_arm_yaw", "r_forearm_pitch", "r_hand_yaw", "r_hand_pitch", "r_hand_roll")
  • 最后 2 个为头部电机数据: head_yaw 和 head_pitch

• 单位:

  • 位置: 弧度 (radian)
  • 速度: 弧度每秒 (radian/s)
  • 加速度: 弧度每平方秒 (radian/s²)
  • 电流: 安培 (A)

5. IMU 数据说明

• gyro: 陀螺仪的角速度，单位弧度每秒（rad/s）
• acc: 加速度计的加速度，单位米每平方秒（m/s²）
• quat: IMU的姿态（orientation）

---

6. 使用示例

import rospy
from kuavo_msgs.msg import sensorsData

def callback(data):
    rospy.loginfo(f"Received sensor data at time: {data.sensor_time}")

# 初始化ROS节点
rospy.init_node('sensor_data_listener')

# 订阅传感器数据话题
rospy.Subscriber('/sensors_data_raw', sensorsData, callback)

# 保持节点运行
rospy.spin()

---

/ik/fk_srv 机器人手臂FK正解

1. 服务描述

/if/fk_srv服务用于机器人手臂FK正解

2. 请求格式

• 类型：motion_capture_ik/fkSrv
• 字段：

字段	类型	描述
q	float64[]	长度为 14 ，内容为手臂关节的角度, 单位弧度

3. 响应格式

• 类型：motion_capture_ik/fkSrv
• 字段：

字段	类型	描述
success	bool	返回参数, 是否成功
hand_poses	twoArmHandPose	返回参数, 正解结果, 具体内容见上述同类型消息

4. 使用示例

import rospy
from motion_capture_ik.srv import fkSrv

# 初始化 ROS 节点
rospy.init_node('example_fk_srv_node')

# 创建服务代理
fk_srv = rospy.ServiceProxy('/ik/fk_srv', fkSrv)

# 创建请求对象(单位：弧度)
joint_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.38, -1.39, -0.29, -0.43, 0.0, -0.17, 0.0]

# 发送请求并接收响应
response = fk_srv_client(joint_angles)

---

/ik/two_arm_hand_pose_cmd_srv 机器人手臂IK逆解

1. 服务描述

/ik/two_arm_hand_pose_cmd_srv服务用于机器人手臂IK逆解

2. 请求格式

• 类型：twoArmHandPoseCmd
• 字段：twoArmHandPoseCmdRequest

字段	类型	描述
hand_poses	twoArmHandPose	双手信息: 末端位置, 末端四元数, 手肘位置等
use_custom_ik_param	bool	是否使用自定义的 IK 参数, 设置为 true 时 会使用消息中的 ik_param 值用于求解
joint_angles_as_q0	bool	是否使用 hand_poses 中的 joint_angles 作为求解时的 q0
ik_param	ikSolveParam	自定义的 IK 求解参数

对于ik_param字段详细描述如下：

字段	类型	描述
major_optimality_tol	float64	snopt 参数, 即主要迭代中优化性的容差, 该参数决定最优性条件的满足程度
major_feasibility_tol	float64	snopt 参数, 即主要迭代中的可行性容差, 用于控制非线性约束
minor_feasibility_tol	float64	snopt 参数, 次要迭代中的可行性容差，主要用于线性化后的模型
major_iterations_limit	float64	snopt 参数, 主要迭代的最大次数
oritation_constraint_tol	float64	姿态约束参数
pos_constraint_tol	float64	位置约束参数, 该参数只会在 pos_cost_weight 大于 0.0 时生效
pos_cost_weight	float64	位置成本参数, 当设置成 0.0 时求解精度要求最高

对于hand_poses字段详细描述如下

字段	类型	描述
pos_xyz	float64[3]	末端期望的位置, 单位 m
quat_xyzw	float64[4]	末端期望的姿态
elbow_pos_xyz	float64[3]	手肘期望的位置, 全设置为 0.0 时忽略该参数
joint_angles	float64[7]	如果 joint_angles_as_q0 为 true, 则使用该值作为求解时的 q0, 单位弧度

3. 响应格式

字段	类型	描述
success	bool	是否成功
with_torso	bool	是否包含躯干
q_arm	float64[]	手臂关节值, 单位弧度
q_torso	float64[]	躯干的关节值
time_cost	float64	求解耗时, 单位 ms
hand_poses	twoArmHandPose	IK 求解结果, 具体内容见上述同类型消息

4. 使用示例

import rospy
from motion_capture_ik.msg import twoArmHandPoseCmd, ikSolveParam
from motion_capture_ik.srv import twoArmHandPoseCmdSrv

# 初始化 ROS 节点
rospy.init_node('example_ik_srv_node')

# 创建服务代理
ik_srv = rospy.ServiceProxy('two_arm_hand_pose_cmd_srv', twoArmHandPoseCmdSrv)

# 使用默认参数
use_custom_ik_param = False
joint_angles_as_q0 = False

# 创建请求对象
request = twoArmHandPoseCmd()
request.use_custom_ik_param = use_custom_ik_param
request.joint_angles_as_q0 = joint_angles_as_q0

# 设置左手末端执行器的位置和姿态
request.hand_poses.left_pose.pos_xyz =  np.array([0.45,0.25,0.11988012])
request.hand_poses.left_pose.quat_xyzw = [0.0,-0.70682518,0.0,0.70738827] # 四元数
request.hand_poses.left_pose.elbow_pos_xyz = np.zeros(3) # 设置成 0.0 时,不会被使用

# 设置右手末端执行器的位置和姿态
request.hand_poses.right_pose.pos_xyz =  np.array([0.45,-0.25,0.11988012])
request.hand_poses.right_pose.quat_xyzw = [0.0,-0.70682518,0.0,0.70738827] # 四元数
request.hand_poses.right_pose.elbow_pos_xyz = np.zeros(3)  # 设置成 0.0 时,不会被使用

# 发送请求并接收响应
response = ik_srv(request)

---

/control_robot_leju_claw 控制机器人夹爪（二指爪）运动

1. 服务描述

/control_robot_leju_claw服务用于机器人夹爪（二指爪）的控制。

• 先决条件: 只有在kuavo.json中配置EndEffectorType为lejuclaw时才会启动该服务。修改配置文件 kuavo.json 的路径为 kuavo-ros-control/src/kuavo_assets/config/kuavo_v42/kuavo.json 。注意要修改机器人型号对应的配置文件。

2. 服务消息类型

• kuavo_msgs/controlLejuClaw

3. 请求格式

字段	类型	描述
data	kuavo_msgs/endEffectorData	请求数据, 夹爪相关的消息

关于 data 字段, 其中 kuavo_msgs/endEffectorData的消息定义如下:

字段	类型	描述
name	string[]	必填项, 数组长度为2, 数据为"left_claw", "right_claw"
position	float64[]	必填项, 数组长度为2, 夹爪目标位置值, 范围为0 ~ 100, 表示行程占比, 0 为张开, 100 为闭合
velocity	float64[]	选填项, 数组长度为2, 夹爪目标速度值, 0 ~ 100, 不填写时默认为50
effort	float64[]	选填项, 数组长度为2, 夹爪目标电流, 单位 A, 不填写时默认为 1.0A

• name: 注意名称只能设置为"left_claw"或 "right_claw"
• position: 范围 0 ~100, 表示行程占比, 0 为张开, 100 为闭合
• velocity : 速度, 默认为 50,
• effort : 力距, 电机不会输出大于该值的电流, 如果给的过小，可能运动效果受限，推荐 1A~2A）, 默认为 1.0 A.

4. 响应格式

字段	类型	描述
success	bool	返回数据, 是否调用成功
message	string	返回数据, 消息

5. 使用示例

#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from kuavo_msgs.srv import controlLejuClaw, controlLejuClawRequest, controlLejuClawResponse

if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('leju_claw_client_node')
    
    # 创建请求对象
    req = controlLejuClawRequest()
    req.data.name = ['left_claw', 'right_claw']
    req.data.position = [90, 90]
    req.data.velocity = [50, 50]
    req.data.effort = [1.0, 1.0]
    
    #确保服务启动
    rospy.wait_for_service('/control_robot_leju_claw')
    #调用服务并获取响应
    control_leju_claw = rospy.ServiceProxy('/control_robot_leju_claw', controlLejuClaw)
    res = control_leju_claw(req)

---

/leju_claw_state 获取机器人夹爪（二指爪）状态

1. 服务描述

/leju_claw_state话题用于发布机器人夹爪（二指爪）的状态，位置，速度，力矩等信息。先决条件: 只有在kuavo.json中配置EndEffectorType为lejuclaw时才会发布该话题。修改方法在/control_robot_leju_claw部分有介绍。

2. 服务消息类型

• kuavo_msgs/lejuClawState

3. 消息字段

字段	类型	描述
state	int8[]	二指夹爪的状态, 数组长度为2, 第一个为左夹爪, 第二个为右夹爪
data	kuavo_msgs/endEffectorData	二指夹爪的位置, 速度, 力距等信息

state 状态值含义:

• -1 : Error, 表示有执行时有错误,
• 0 : Unknown, 初始化时默认的状态,
• 1 : Moving, 表示夹爪正在执行, 移动中,
• 2 : Reached, 表示夹爪已经执行到达期望的位置,
• 3 : Grabbed, 表示夹爪抓取到物品.

关于 data 字段中 kuavo_msgs/endEffectorData的消息在/control_robot_leju_claw部分有介绍。

4. 使用示例：

#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from kuavo_msgs.msg import lejuClawState

# 定义回调函数
def leju_calw_state_callback(msg):
    claw_state = msg.state
    # 打印当前夹爪状态
    rospy.loginfo(f"Current claw state: left={claw_state[0]}, right={claw_state[1]}")

if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('leju_claw_state_node')

    # 创建订阅者，监听 /leju_claw_state 话题
    claw_state_sub = rospy.Subscriber('/leju_claw_state', lejuClawState, leju_calw_state_callback)

    # 保持节点运行
    rospy.spin()

---

/leju_quest_bone_poses VR骨骼位姿数据

1. 功能描述

/leju_quest_bone_poses 话题用于发布Quest3 VR设备追踪的骨骼位姿数据，包括26个骨骼节点的位置和方向信息。

2. 消息类型

• 类型: noitom_hi5_hand_udp_python/PoseInfoList

3. 消息字段

• timestamp_ms (int): 时间戳
• is_high_confidence (bool): 数据置信度标志
• is_hand_tracking (bool): 手部追踪状态标志
• poses (PoseInfo[]): 包含所有骨骼的位姿数据列表，每个骨骼包含：
  • position (geometry_msgs/Point): 骨骼位置 (x, y, z)
  • orientation (geometry_msgs/Quaternion): 骨骼方向四元数 (x, y, z, w)

4. 骨骼列表（共26个骨骼）

[
    # 手臂
    "LeftArmUpper", "LeftArmLower",      # 左臂上部和下部
    "RightArmUpper", "RightArmLower",    # 右臂上部和下部
    
    # 手掌
    "LeftHandPalm", "RightHandPalm",     # 左右手掌
    
    # 左手指
    "LeftHandThumbMetacarpal",           # 左拇指掌骨
    "LeftHandThumbProximal",             # 左拇指近节
    "LeftHandThumbDistal",               # 左拇指远节
    "LeftHandThumbTip",                  # 左拇指指尖
    "LeftHandIndexTip",                  # 左食指指尖
    "LeftHandMiddleTip",                 # 左中指指尖
    "LeftHandRingTip",                   # 左无名指指尖
    "LeftHandLittleTip",                 # 左小指指尖
    
    # 右手指
    "RightHandThumbMetacarpal",          # 右拇指掌骨
    "RightHandThumbProximal",            # 右拇指近节
    "RightHandThumbDistal",              # 右拇指远节
    "RightHandThumbTip",                 # 右拇指指尖
    "RightHandIndexTip",                 # 右食指指尖
    "RightHandMiddleTip",                # 右中指指尖
    "RightHandRingTip",                  # 右无名指指尖
    "RightHandLittleTip",                # 右小指指尖
    
    # 躯干
    "Root", "Chest", "Neck", "Head"      # 根部、胸部、颈部、头部
]

5. 使用示例

import rospy
from noitom_hi5_hand_udp_python.msg import PoseInfoList

def bone_poses_callback(data):
    # 处理接收到的骨骼位姿数据
    print(f"Timestamp: {data.timestamp_ms}")
    print(f"High confidence: {data.is_high_confidence}")
    print(f"Hand tracking: {data.is_hand_tracking}")
    print(f"Number of poses: {len(data.poses)}")

# 初始化ROS节点
rospy.init_node('bone_poses_listener')

# 订阅骨骼位姿数据话题
rospy.Subscriber('/leju_quest_bone_poses', PoseInfoList, bone_poses_callback)

# 保持节点运行
rospy.spin()