机器人控制

1. 节点说明

节点

包

职责

joint_manager

ti5_control

接收关节命令、处理动作服务、下发底层、发布关节状态与运控状态；内嵌 ti5/arm_movej、ti5/arm_movel；T170C / T170C_2_0 另内嵌 ti5/waist_movej、ti5/waist_movel、ti5/waist_fk、ti5/waist_ik（折叠 3 轴末端运动学）。MoveJ/MoveL execute 在独立线程下发轨迹，进程内使用 MultiThreadedExecutor，轨迹执行期间 jointstate_list 仍持续刷新

arm_ik/fk（内嵌）

ti5_control/joint_manager

提供双臂 IK/FK 服务（与轨迹服务同进程）

joint_cmd_client（示例）

ti5_control_client

示例：发布关节命令并调用动作/运动学服务

2. 话题定义

joint_manager 核心话题：

  • 订阅：ti5/jointcmd_list（ti5_control/msg/JointCmdList）

  • 发布：

    • ti5/jointstate_list（ti5_control/msg/JointStateList）

    • ti5/motion_control_status（ti5_control/msg/MotionControlStatus，默认 50ms 周期）

关键消息字段（对齐 get_joint_states() / get_motion_control_status() 口径）：

  • ti5_control/msg/JointCommand

    • joint_id：关节 ID（需与 robot_joint_config.yaml 中 app_id 对齐）

    • mode：控制模式（MODE_POS=0，MODE_PT=1）

    • angle_unit：单位（ANGLE_RADIAN=0，ANGLE_DEGREE=1）

    • value：目标值（由 angle_unit 决定单位）

  • ti5_control/msg/JointCmdList

    • source：指令来源（SOURCE_EXTERNAL=0 外部客户端；SOURCE_PLANNER=1 内部轨迹规划器，不触发双臂模式切换）

    • commands：JointCommand[]

  • ti5_control/msg/JointState（单关节反馈）

    • joint_id、name、mode

    • motor_position：电机位置反馈（弧度 rad）

    • motor_velocity：电机速度反馈（rad/s；当前实现多为 0，尚未周期查询 CSP）

    • motor_current：电机电流反馈（A；当前实现多为 0）

    • error_code：故障/报警码（0 正常；位图，见下表）

  • ti5_control/msg/JointStateList

    • seq：单调递增发布序号（每条消息 +1，用于检测话题是否存活）

    • timestamp_ns：最近一次电机 CAN 反馈时刻（CLOCK_REALTIME 纳秒）

    • nums：关节数量

    • states：JointState[]

  • ti5_control/msg/MotionControlStatus（运控总览，见下文 运控状态与规划反馈）

  • ti5_control/msg/MotionFeedback（嵌套在 MotionControlStatus.active_motion 及服务响应中）

    • motion_id、motion_state（IDLE/ACCEPTED/RUNNING/REACHED/FAILED/CANCELED）

    • error_code、error_msg、timestamp_ns、life_time_ms、source

关节映射说明：

  • 控制命令中的 joint_id 必须与当前型号配置中的 app_id 一一对应；

  • 关节配置路径：ti5_control/config/<TYPE>/robot_joint_config.yaml；

  • 型号与配置混用会导致关节映射错误。

控制周期（来自 robot_joint_config.yaml 的 timers 段；各型号以 yaml 为准）：

  • state_publish_period_ms：状态发布周期（常见 10ms）

  • state_query_period_ms：CAN 位置查询周期（T170C 等为 20ms；代码缺省回退值为 2000ms，以 yaml 覆盖为准）

  • pending_command_period_ms：命令下发周期（常见 5ms）

关节反馈新鲜度（客户端必读）：

  • timestamp_age = now_ns - jointstate_list.timestamp_ns（秒）

  • seq 在等待到位期间应持续递增

  • 建议阈值：timestamp_age < 1.0s 且 seq 在增长；否则视为反馈停更/过期，不应继续等待到位

  • motion_control_status 同步携带 jointstate_seq / jointstate_timestamp_ns；顶层 error_code=-1001 表示反馈停更

JointState.error_code 电机故障位（CAN GET_FAULT 位图）：

位

十进制值

含义

bit0

1

电机内部软件错误（如电机运行时写入 FLASH 等）

bit1

2

过压

bit2

4

欠压

bit4

16

启动错误

bit5

32

速度反馈错误

bit6

64

过流

bit7

128

软件错误（如电机运行时写入 FLASH 等）

bit16

65536

编码器通讯错误

bit17

131072

电机温度过高

bit18

262144

电路板温度过高

bit19

524288

驱动芯片错误/电流过载

bit20

1048576

EtherCAT 通讯错误（连接 EC 后断开 EC 会产生；重启主站或重新上电消除）

bit21

2097152

位置追随误差过大（仅位置/轮廓位置模式；CAN 默认不开启此功能）

—

-1

非电机位图：joint_manager CAN 反馈超时

多 bit 可同时置位（如 error_code=393216 表示 bit17+bit18：电机温度过高且电路板温度过高）。清告警：ti5/joint_action action_type=3（ACTION_CLEAR_ALARM）。

3. 运控状态与规划反馈

3.1 运控模式（MotionControlStatus）

话题：ti5/motion_control_status（ti5_control/msg/MotionControlStatus）

字段

含义

mode

全局运控模式：0 停止、1 伺服、2 路径规划（max(arm_mode, body_mode)）

arm_mode

双臂模式（BodyMove 双臂 / MoveJ/MoveL execute / 外部 JointCmdList 控臂时变化）

body_mode

身体其它部位规划模式（BodyMove 头/腰/Fold、waist_move* execute 时置 2）

error_code / error_msg

运控层健康状态：关节故障码或反馈停更（-1001）

active_motion

当前活动规划运动的 MotionFeedback

jointstate_seq / jointstate_timestamp_ns

与 jointstate_list 对齐

motion_state 状态机（MotionFeedback）：

值

名称

含义

0

IDLE

无活动规划

1

ACCEPTED

已接受（极短，服务返回前常已进入 RUNNING）

2

RUNNING

轨迹下发中或等待关节反馈到位

3

REACHED

参与关节 jointstate 反馈均在容差内并稳定

4

FAILED

规划/下发失败、到位超时、反馈停更或关节故障

5

CANCELED

被新规划或 release_motion_control 取消

到位判定（REACHED）：轨迹指令发完后仍保持 RUNNING，由 joint_manager 按 jointstate_list 的 motor_position 与目标角比较。参数写在各机型 config/<TYPE>/robot_joint_config.yaml 的 motion_reach 段（启动时加载；ros2 -p 可覆盖）：

motion_reach:
  tolerance_cnt: 100          # 电机 cnt 容差
  stable_sec: 0.15            # 容差内连续稳定多久判 REACHED（秒）
  timeout_margin_sec: 10.0     # 超过 life_time_ms 后再等待多久判 FAILED（秒）

临时覆盖示例：

ros2 run ti5_control joint_manager --type T170C_2_0 --ros-args -p motion_reach_tolerance_cnt:=150

规划类接口（BodyMove、ArmMoveJ/L execute、WaistMoveJ/L execute）：

  • 服务响应与服务端 active_motion 均含：motion_id、motion_state、error_code、timestamp_ns、life_time_ms

  • success=true 表示 已接受规划（通常已进入 RUNNING），不等于已到位

  • 到位条件：active_motion.motion_state == 3（REACHED），且 jointstate 反馈新鲜（seq 递增、timestamp_age < 1s）

  • 到位失败：active_motion.error_code 可能为 -1002（超时）、-1003（关节故障）、-1001（反馈停更）

伺服类接口（JointCmdList）：

  • 无单条指令完成反馈；通过 jointstate 的 seq/timestamp_ns/关节误差判断状态

  • 外部 JointCmdList（source=0）若涉及双臂关节：自动停止当前双臂规划、将 arm_mode 切为伺服（1），进行中 motion 标记为 CANCELED

显式释放运控（ti5/release_motion_control，ReleaseMotionControl.srv）：

ros2 service call /ti5/release_motion_control ti5_control/srv/ReleaseMotionControl \
  "{release_planning: true, release_servo_arms: true}"

  • release_planning：停止轨迹规划，arm_mode/body_mode 中处于规划的部分回到停止

  • release_servo_arms：清除双臂 JointCmd 缓存（received=false）

轮询示例：

ros2 topic echo /ti5/motion_control_status --field mode --field arm_mode --field body_mode --field error_code --field active_motion --field jointstate_seq

# 监控关节反馈新鲜度
ros2 topic echo /ti5/jointstate_list --field seq --field timestamp_ns

4. 服务接口

4.1 服务列表

面向调用方的服务总览（启动 joint_manager 后可直接调用）：

  • 基础控制：

    • ti5/joint_action（ti5_control/srv/JointAction）

    • ti5/body_move（ti5_control/srv/BodyMove）

    • ti5/release_motion_control（ti5_control/srv/ReleaseMotionControl）

  • 双臂末端控制：

    • ti5/arm_ik（ti5_control/srv/ArmIK）

    • ti5/arm_fk（ti5_control/srv/ArmFK）

    • ti5/arm_movej（ti5_control/srv/ArmMoveJ）

    • ti5/arm_movel（ti5_control/srv/ArmMoveL）

  • T170C / T170C_2_0 折叠机构末端控制（joint_manager --type T170C 或 T170C_2_0 时注册）：

    • ti5/waist_fk（ti5_control/srv/WaistFK）

    • ti5/waist_ik（ti5_control/srv/WaistIK）

    • ti5/waist_movej（ti5_control/srv/WaistMoveJ）

    • ti5/waist_movel（ti5_control/srv/WaistMoveL）

4.2 JointAction 字段

JointAction.srv 关键字段：

  • action_type：动作编号（见下方“动作编号说明”）

  • velocity：动作速度系数（当前部分动作会忽略该值，详见动作说明）

  • param1/param2/param3/param4：动作附加参数

    • ACTION_SHIFT_TO 时 param1 表示目标高度（单位 mm，范围 0~175，仅 T170D 生效）

动作编号说明（填写到 action_type）：

  • ACTION_HOME = 0

  • ACTION_SHIFT_TO = 2（仅 T170D 机型）

  • ACTION_CLEAR_ALARM = 3

  • ACTION_MOTION_HI = 4

4.3 BodyMove 字段

BodyMove.srv 关键字段：

  • use_arms：是否执行双臂目标（true 时会同时执行右臂与左臂，两个数组都需要给出）

  • use_neck：是否执行头部（Neck）目标

  • use_waist：是否执行腰部（Waist）目标

  • use_fold：是否执行折叠机构（Fold；要求配置中存在 Fold 组，如 T170C / T170C_2_0）

  • right_arm_radians[7] / left_arm_radians[7]：各臂 7 个关节的目标角（弧度 rad）

  • neck_radians[3] / waist_radians[3]：头/腰关节目标角（弧度 rad；实际使用按型号配置分组大小取前 N 个，剩余可填 0）

  • fold_radians[4]：Fold 目标角（弧度 rad），按配置中 Fold 关节顺序取前 N 个（接口固定 4 维，多余填 0）。T170C：通常 Fold_P3,Fold_P2,Fold_P1（3 轴）；T170C_2_0：Fold_P1,Fold_P2（2 轴，第三轴蹲起见 Waist 的 WAIST_P / ti5/waist_*）

  • velocity：轨迹规划速度（弧度每秒 rad/s；<=0 时服务端使用默认值 0.5）

响应字段（规划反馈）：

  • success：true 表示已接受规划（ACCEPTED），不代表已到位

  • motion_id、motion_state、error_code、timestamp_ns、life_time_ms

  • 到位请轮询 ti5/motion_control_status 中 active_motion.motion_state == 3（REACHED）

说明：

  • 无 Fold 组的机型（如 T140A/T170D）若将 use_fold 置 true，服务将返回错误信息 Fold joint group not found in robot_joint_cfg。

  • T170C：BodyMove.use_waist 只动 Waist 组（通常 WAIST_Y）；折叠 3 轴末端请用 ti5/waist_*。

  • T170C_2_0：use_waist 动 WAIST_Y/P/R；蹲起三轴（Fold_P1/P2 + WAIST_P）请用 ti5/waist_*，不要用 use_fold 代替整段运动学。

ros2 service call 可写精简请求：未出现的字段按类型默认（bool 为 false，数组为 0）。因此只动 Fold 时不必写 use_arms、左右臂数组等，只要启用 use_fold 并给出 fold_radians 即可（服务端仅处理 use_* 为 true 的组）。

4.4 ArmIK / ArmFK 字段

ArmIK.srv / ArmFK.srv 关键字段：

  • robot_type：机器人型号字符串（用于选择对应 IK/FK 解算参数；示例 T140A、T170C、T170C_2_0、T170D）

  • arm：控制对象手臂：left 或 right

  • target_x/y/z（IK 输入）：末端位置坐标（单位以工程定义为准，通常为 meter）

  • target_rx/ry/rz（IK 输入）：末端姿态（rx/ry/rz，roll/pitch/yaw），单位为弧度 rad

  • j2（IK 输入，兼容）：无 live/7 轴种子时作数值 IK 初值；0 为默认构型（左 +10°、右 -10°，优先于 q2d）

  • use_q2d、q2d（IK 输入）：与 arm_movel 一致；use_q2d: true 时走 ik_analytic 锁定 J2；q2d 非 0 用指定角，q2d: 0 用种子/current J2（索引 1）

  • use_live_joint_state_for_seed（IK）：为 true 时从 ti5/jointstate_list 读当前臂 7 轴作 IK 初值（由 joint_manager 内置缓存提供）

  • joint_angles_seed[]（IK）：use_live=false 且长度为 7 时作 IK 初值

  • joint_angles[7]（IK 输出 / FK 输入）：手臂 7 个关节角（弧度 rad）

    • 关节顺序与 ti5_control/config/<TYPE>/robot_joint_config.yaml 中 Right_Arm/Left_Arm 的关节顺序一致

  • use_live_joint_state（FK）：为 true 时从 ti5/jointstate_list 读当前臂 7 轴，忽略 joint_angles

  • joint_angles[7]（FK）：use_live_joint_state=false 时必填

  • x/y/z/rx/ry/rz（FK 输出）：由关节角计算得到的末端位姿（位置 m，姿态 ZYX rad）

4.5 ArmMoveJ / ArmMoveL 字段

由 joint_manager 进程提供（与 SDK ITFTwoArm::planMoveJ / planMoveL 一致）。robot_type 必须与启动 joint_manager 时的 --type 一致（如 T170C_2_0）；请求里留空则使用启动默认值。

IK 与 planMoveJ/planMoveL 在独立工作线程执行；jointstate_list 的 query/publish 定时器在 Reentrant 回调组，MoveJ/MoveL 服务等待期间仍可刷新（需 MultiThreadedExecutor）。

ArmMoveL.srv（笛卡尔直线）关键字段：

  • robot_type、arm（left / right）

  • use_live_joint_state：为 true 时规划前从真机读当前关节（queryPos + getBodyRadian），忽略 joint_angles_current

  • joint_angles_current[7]：use_live_joint_state=false 时必填，当前关节角（弧度）

  • target_x/y/z、target_rx/ry/rz：目标末端位姿（位置 m，姿态 rad）

  • vel：规划速度（>0）

  • use_q2d、q2d：锁定 J2 冗余角（use_q2d: true 时生效）；q2d 非 0 用指定角，q2d: 0 用规划起点 qc 的当前 J2（索引 1）

  • execute：为 true 时规划成功后由 joint_manager 按周期下发轨迹

  • execute_dt_ms：下发周期（毫秒）；<=0 时用节点默认（通常来自 robot_joint_config.yaml 的 pending_command_period_ms）

  • skip_traj_in_response：为 true 时不返回 traj_data 大数组（仅保留 traj_rows/traj_cols），联调执行时建议 true

ArmMoveJ.srv（关节空间）关键字段：

  • use_live_joint_state_for_start：为 true 时从真机读起点 q_start（q_start 可传空数组 []）

  • q_start[]：use_live_joint_state_for_start=false 时必须 7 个关节角（弧度）

  • 终点二选一：

    • use_pose_target: true：用 target_x/y/z/rx/ry/rz（内部先 IK 得 q_end），q_end 可 []

    • use_pose_target: false：必须提供 q_end 共 7 个关节角

  • j2：兼容初值；use_q2d、q2d：位姿终点 IK 时锁定 J2（语义同 arm_ik / arm_movel）

  • vel、execute、execute_dt_ms、skip_traj_in_response：含义同 ArmMoveL

响应字段（execute=true 时）：motion_id、motion_state、error_code、timestamp_ns、life_time_ms（语义同 BodyMove）

说明：双臂运动学 model / DH / tool / joint_axis 写在各机型 config/<TYPE>/robot_joint_config.yaml 的 arm_itf_kinematics（如 T170C → T7_170C，T170C_2_0 → T7_170C_2_0）；与 Robot_Model.yaml 顶层键一致。

4.6 T170C / T170C_2_0 折叠机构 Waist FK / IK / MoveJ / MoveL

适用范围：joint_manager --type T170C 或 T170C_2_0（请求 robot_type 与启动 --type 一致，可留空）。运动学为 3-DoF（TI5_WAIST）。

机型差异：

项

T170C

T170C_2_0

配置目录

config/T170C/

config/T170C_2_0/

三轴执行器

Fold：Fold_P3/P2/P1

Fold_P1、Fold_P2 + Waist：WAIST_P

waist_* 关节数组顺序

yaml：P3,P2,P1

运动学：P1,P2,WAIST_P

连杆长度

waist_fold_kinematics：a2/a3（默认 0.300 / 0.4225）

同字段（默认 0.260 / 0.400）

双臂 model

T7_170C

T7_170C_2_0

连杆长度写在对应 yaml 的 waist_fold_kinematics.a2 / a3（单位 m），启动时加载。

单位：关节角为 电机侧弧度 rad；末端位姿为 米 m + ZYX 欧拉 rad。

WaistFK.srv / WaistIK.srv / WaistMoveJ.srv / WaistMoveL.srv：

  • robot_type：T170C 或 T170C_2_0（与 --type 一致）

  • live 读角 / 种子 / q_end / target_* / vel / execute 等语义同双臂 arm_*；关节维数为 3

  • execute: true 时仅驱动上述三轴

控制链路（简要）：

  1. 上层程序发布 ti5/jointcmd_list（建议 source=0）或调用动作/规划服务（含 ti5/arm_*、ti5/waist_*、ti5/body_move）；

  2. joint_manager 按型号配置映射 joint_id -> app_id 并执行控制（waist_move* 仅驱动折叠三轴）；

  3. 系统周期发布 ti5/jointstate_list（含 seq/timestamp_ns）与 ti5/motion_control_status；use_live_* 类服务从关节缓存/反馈读取当前角；

  4. 同一双臂动作链路内避免混用 BodyMove 与外部 JointCmdList；必须切换时先 release_motion_control 或依赖自动模式切换（外部 JointCmd 会取消进行中的双臂规划）。

5. 示例程序

提示：若通过 ros2 launch ti5_bringup ... namespace:=/robot1 启动，则调用服务/发布话题时需要带命名空间，例如 /robot1/ti5/joint_action。本文示例默认无命名空间。

5.1 动作服务示例（JointAction）

ti5/joint_action（JointAction.srv 各动作类型示例）：

ACTION_HOME（action_type: 0，归零）：

ros2 service call /ti5/joint_action ti5_control/srv/JointAction "{action_type: 0, velocity: 1.0, param1: 0, param2: 0, param3: 0.0, param4: 0.0}"

ACTION_SHIFT_TO（action_type: 2，升降到位；仅 T170D 机型。param1 为高度 mm，范围 0~175）：

ros2 service call /ti5/joint_action ti5_control/srv/JointAction "{action_type: 2, velocity: 1.0, param1: 100, param2: 0, param3: 0.0, param4: 0.0}"

ACTION_CLEAR_ALARM（action_type: 3，清除告警）：

ros2 service call /ti5/joint_action ti5_control/srv/JointAction "{action_type: 3, velocity: 1.0, param1: 0, param2: 0, param3: 0.0, param4: 0.0}"

ACTION_MOTION_HI（action_type: 4，按 motion_hi.csv 播放轨迹；当前实现不按 velocity 缩放时间轴）：

ros2 service call /ti5/joint_action ti5_control/srv/JointAction "{action_type: 4, velocity: 1.0, param1: 0, param2: 0, param3: 0.0, param4: 0.0}"

5.2 轨迹插补控制与关节直发示例（BodyMove / JointCmdList）

两者区别（核心）：

  • BodyMove：服务端会做轨迹规划与插补（五次多项式轨迹），再连续下发关节命令；适合平滑动作与多部位联动（含 Fold，若该机型配置中有 Fold 组）。调用后请轮询 ti5/motion_control_status 等待 active_motion.motion_state == 3（REACHED）。

  • JointCmdList：你直接发布关节目标，按消息内容立即生效，不经过 BodyMove 的轨迹规划；适合单关节/单手臂快速调试。外部下发请设 source: 0。

  • 双臂抓取流程建议：同一链路内统一用一种控制路径；BodyMove 后若切外部伺服，需关注 arm_mode 与 ti5/release_motion_control。

BodyMove 示例（带轨迹插补）：

只动头（Neck）（示例：点头 0.20rad）：

ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{use_neck: true, neck_radians: [0.20, 0.0, 0.0], velocity: 1.0}"

只动腰（Waist）（示例：扭腰 0.20rad）：

ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{use_waist: true, waist_radians: [0.20, 0.0, 0.0], velocity: 1.0}"

只动双臂（示例：T140A 伸直位）：

ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{use_arms: true, right_arm_radians: [0.0, 1.3, 0.0, 0.0, 0.0, 0.0, 0.0], left_arm_radians: [-0.0, -1.3, 0.0, 0.0, 0.0, 0.0, 0.0], velocity: 1.0}"

全身联动（双臂 + 头 + 腰，示例）：

ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{use_arms: true, right_arm_radians: [0.0, 1.3, 0.0, 0.0, 0.0, 0.0, 0.0], left_arm_radians: [-0.0, -1.3, 0.0, 0.0, 0.0, 0.0, 0.0], use_neck: true, neck_radians: [0.10, 0.0, 0.0], use_waist: true, waist_radians: [0.15, 0.0, 0.0], velocity: 1.0}"

仅动 折叠机构（Fold）（按 yaml Fold 顺序填 fold_radians）：

T170C_2_0（Fold：Fold_P1,Fold_P2；第三轴在 Waist 的 WAIST_P，waist_radians 顺序：WAIST_Y,WAIST_P,WAIST_R）：

# 零位（Fold + WAIST_P 一并回零）
ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{velocity: 0.5, use_fold: true, fold_radians: [0.0, 0.0, 0.0, 0.0], use_waist: true, waist_radians: [0.0, 0.0, 0.0]}"

# 半蹲（与 config 中 squat_half 一致）
ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{velocity: 0.4, use_fold: true, fold_radians: [-0.3927, 0.6894, 0.0, 0.0], use_waist: true, waist_radians: [0.0, 0.2967, 0.0]}"

BodyMove 是按组分别给目标角；若要做蹲起末端 FK/IK/直线规划，用 ti5/waist_*（一次管三轴）。

T170C（顺序：Fold_P3,Fold_P2,Fold_P1，第 4 维填 0）：

# 零位
ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{velocity: 0.5, use_fold: true, fold_radians: [0.0, 0.0, 0.0, 0.0]}"

# 半折叠（现场按限位微调）
ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{velocity: 0.4, use_fold: true, fold_radians: [-0.59, -1.38, 0.79, 0.0]}"

# 全折叠（现场按限位微调）
ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{velocity: 0.4, use_fold: true, fold_radians: [-1.18, -2.76, 1.58, 0.0]}"

JointCmdList 示例（关节目标直发）：

只动右臂/只动左臂（使用 ti5/jointcmd_list，只下发对应手臂关节的 joint_id）：

示例以 T140A 为例（robot_joint_config.yaml）：

  • 右臂 7 关节：joint_id=16..22（按 Right_Arm 配置顺序）

  • 左臂 7 关节：joint_id=23..29（按 Left_Arm 配置顺序）

右臂伸直（右臂关节角 rad）：

ros2 topic pub --once ti5/jointcmd_list ti5_control/msg/JointCmdList "{source: 0, commands: [{joint_id: 16, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 17, mode: 0, angle_unit: 0, value: 1.3}, {joint_id: 18, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 19, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 20, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 21, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 22, mode: 0, angle_unit: 0, value: 0.0}]}"

左臂伸直（左臂关节角 rad）：

ros2 topic pub --once ti5/jointcmd_list ti5_control/msg/JointCmdList "{source: 0, commands: [{joint_id: 23, mode: 0, angle_unit: 0, value: -0.0}, {joint_id: 24, mode: 0, angle_unit: 0, value: -1.3}, {joint_id: 25, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 26, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 27, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 28, mode: 0, angle_unit: 0, value: 0.0}, {joint_id: 29, mode: 0, angle_unit: 0, value: 0.0}]}"

5.3 双臂末端控制示例（包含 IK / FK / MoveJ / MoveL）

前置：已启动 joint_manager，且型号一致，例如：

./script/run_ti5_services.sh --type T170C_2_0
# 或
ros2 run ti5_control joint_manager --type=T170C_2_0

推荐顺序：FK（核对当前姿态） -> IK（验证可达） -> MoveJ（快速到位） -> MoveL（末端直线）。

FK（正解）

正解 FK（显式关节角）：

ros2 service call /ti5/arm_fk ti5_control/srv/ArmFK "{robot_type: 'T140A', arm: 'right', use_live_joint_state: false, joint_angles: [0.0, 1.3, 0.0, 0.0, 0.0, 0.0, 0.0]}"

正解 FK（当前真机角度，需 ti5/jointstate_list 有数据）：

ros2 service call /ti5/arm_fk ti5_control/srv/ArmFK "{robot_type: 'T170C_2_0', arm: 'right', use_live_joint_state: true, joint_angles: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"

IK（逆解）

逆解 IK（仅解算、不下发；示例：T170C_2_0 右臂）：

j2 传 0.0 且 use_q2d: false 时表示数值 IK 默认构型（左臂 +10deg，右臂 -10deg）。锁定当前 J2 逆解：use_q2d: true, q2d: 0.0, use_live_joint_state_for_seed: true。

ros2 service call /ti5/arm_ik ti5_control/srv/ArmIK "{robot_type: 'T170C_2_0', arm: 'right', target_x: 0.40, target_y: -0.24, target_z: -0.26, target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, use_q2d: true, q2d: 0.0, use_live_joint_state_for_seed: true}"

仅数值 IK（不锁 J2）示例：

ros2 service call /ti5/arm_ik ti5_control/srv/ArmIK "{robot_type: 'T170C_2_0', arm: 'right', target_x: 0.40, target_y: -0.24, target_z: -0.26, target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, j2: 0.0}"

MoveL（笛卡尔直线，ti5/arm_movel）

规划 + 执行一体：execute: true 时由 joint_manager 按 execute_dt_ms 下发轨迹。use_live_joint_state: true 时以真机当前关节为规划起点。

右臂目标 x=0.2 m（较近）：

ros2 service call /ti5/arm_movel ti5_control/srv/ArmMoveL \
"{robot_type: 'T170C_2_0', arm: 'right', \
use_live_joint_state: true, \
joint_angles_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], \
target_x: 0.2, target_y: -0.24, target_z: -0.26, \
target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, \
vel: 0.15, use_q2d: false, q2d: 0.0, \
execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

右臂目标 x=0.4 m（较远）：

ros2 service call /ti5/arm_movel ti5_control/srv/ArmMoveL \
"{robot_type: 'T170C_2_0', arm: 'right', \
use_live_joint_state: true, \
joint_angles_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], \
target_x: 0.4, target_y: -0.24, target_z: -0.26, \
target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, \
vel: 0.15, use_q2d: false, q2d: 0.0, \
execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

锁定当前 J2 做直线（use_q2d: true 且 q2d: 0 时自动取 qc 索引 1 的当前角）：

ros2 service call /ti5/arm_movel ti5_control/srv/ArmMoveL \
"{robot_type: 'T170C_2_0', arm: 'right', \
use_live_joint_state: true, \
joint_angles_current: [], \
target_x: 0.3, target_y: -0.24, target_z: -0.26, \
target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, \
vel: 0.15, use_q2d: true, q2d: 0.0, \
execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

MoveJ（关节空间，ti5/arm_movej，终点为位姿时内部逆解）

use_pose_target: true：终点由 target_* 指定，服务端先 IK 再 planMoveJ。起点由 use_live_joint_state_for_start: true 从真机读取。

右臂终点 x=0.40 m：

ros2 service call /ti5/arm_movej ti5_control/srv/ArmMoveJ \
"{robot_type: 'T170C_2_0', arm: 'right', \
use_live_joint_state_for_start: true, q_start: [], \
use_pose_target: true, \
target_x: 0.40, target_y: -0.24, target_z: -0.26, \
target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, \
q_end: [], j2: 0.0, vel: 0.15, \
execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

右臂终点 x=0.20 m：

ros2 service call /ti5/arm_movej ti5_control/srv/ArmMoveJ \
"{robot_type: 'T170C_2_0', arm: 'right', \
use_live_joint_state_for_start: true, q_start: [], \
use_pose_target: true, \
target_x: 0.20, target_y: -0.24, target_z: -0.26, \
target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, \
q_end: [], j2: 0.0, vel: 0.15, \
execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

5.4 T170C / T170C_2_0 折叠机构末端控制示例（ti5/waist_*）

前置：joint_manager --type T170C_2_0（T170C 同理，改 robot_type 即可）。q_end 顺序为运动学列：Fold_P1, Fold_P2, WAIST_P。笛卡尔位姿请先用本机 waist_fk 核对后再填（不同型号连杆长度不同）。

推荐顺序：FK → IK → MoveJ → MoveL。

FK（正解）

ros2 service call /ti5/waist_fk ti5_control/srv/WaistFK "{robot_type: 'T170C_2_0', use_live_joint_state: true, joint_angles: [0.0, 0.0, 0.0]}"

IK（逆解）

ros2 service call /ti5/waist_ik ti5_control/srv/WaistIK "{robot_type: 'T170C_2_0', use_live_joint_state_for_seed: true, target_x: -0.136861, target_y: 0.0, target_z: 0.202175, target_rx: 1.5708, target_ry: -1.57022, target_rz: 0.0}"

MoveL（笛卡尔直线）

ros2 service call /ti5/waist_movel ti5_control/srv/WaistMoveL "{robot_type: 'T170C_2_0', use_live_joint_state: true, joint_angles_current: [0.0, 0.0, 0.0], target_x: -0.136861, target_y: 0.0, target_z: 0.202175, target_rx: 1.5708, target_ry: -1.57022, target_rz: 0.0, vel: 0.1, execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

MoveJ（关节空间）

ros2 service call /ti5/waist_movej ti5_control/srv/WaistMoveJ "{robot_type: 'T170C_2_0', use_live_joint_state_for_start: true, q_start: [], use_pose_target: false, q_end: [0.0, 0.0, 0.0], vel: 0.1, execute: false, execute_dt_ms: 5.0, skip_traj_in_response: true}"

典型流程：IK → MoveJ：将 waist_ik 返回的 joint_angles 填入 waist_movej 的 q_end，设 execute: true。

ros2 service call /ti5/waist_movej ti5_control/srv/WaistMoveJ "{robot_type: 'T170C_2_0', use_live_joint_state_for_start: true, q_start: [], use_pose_target: false, q_end: [0.20, -0.10, 0.30], vel: 0.1, execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

ros2 service call /ti5/waist_movej ti5_control/srv/WaistMoveJ "{robot_type: 'T170C_2_0', use_live_joint_state_for_start: true, q_start: [], use_pose_target: true, target_x: -0.136861, target_y: 0.0, target_z: 0.202175, target_rx: 1.5708, target_ry: -1.57022, target_rz: 0.0, q_end: [], vel: 0.1, execute: true, execute_dt_ms: 5.0, skip_traj_in_response: true}"

5.5 轨迹控制与运动学结合（BodyMove + IK）

仅需关节角、不需 SDK 轨迹规划时，可先 IK 再 BodyMove：

  1. 分别调用左右臂 IK，得到各 7 个关节角；

  2. 将结果填入 BodyMove 的 right_arm_radians / left_arm_radians 一次下发。

ros2 service call /ti5/arm_ik ti5_control/srv/ArmIK "{robot_type: 'T170C_2_0', arm: 'left', target_x: 0.35, target_y: 0.15, target_z: -0.20, target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, j2: 0.0}"
ros2 service call /ti5/arm_ik ti5_control/srv/ArmIK "{robot_type: 'T170C_2_0', arm: 'right', target_x: 0.35, target_y: -0.15, target_z: -0.20, target_rx: 0.0, target_ry: 0.0, target_rz: 0.0, j2: 0.0}"
ros2 service call /ti5/body_move ti5_control/srv/BodyMove "{use_arms: true, right_arm_radians: [<右臂IK返回7个角>], left_arm_radians: [<左臂IK返回7个角>], velocity: 1.0}"

5.6 二次开发示例（ti5_control_client）

  • 该包提供可直接编译运行的客户端示例（joint_cmd_client），适合客户做接口联调与二次开发起点。

  • 可用 ./script/run_ti5_client.sh --type T170C_2_0（或包内 ./script/run_joint_cmd.sh --type T170C_2_0）启动示例节点。

  • 示例节点仅按 src/ti5_control_client/src/config/<型号>/example_actions.yaml 顺序循环；支持 joint_action、body_move、arm_ik/fk/movej/movel、waist_*（T170C / T170C_2_0）、jointcmd 等，MoveJ/MoveL 可在 YAML 中设 execute: true 真机下发。字段说明见同目录 config/README.md。

  • src/reference/ 含与 ti5_control 同步的 msg/srv 只读副本（含 ArmMoveJ、WaistMoveJ 等）；字段以已安装的 ti5_control 为准。

  • robot_type 必须与 joint_manager --type 一致；无需也不要再启动 arm_ik_node（源码中仍保留该可执行文件供调试，交付入口仅为 run_ti5_services.sh → joint_manager）。

  • ti5/waist_* 仅由 joint_manager 在 --type T170C / T170C_2_0 时注册。

  • 修改本包后需在工作空间根目录 colcon build --packages-select ti5_control_client 并重新 source install/setup.bash。