机器人控制

1. 节点说明

节点

包

职责

joint_manager

ti5_control

接收关节命令、处理动作服务、下发底层、发布关节状态

arm_ik_node

ti5_control

提供 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_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

    • commands: JointCommand[]

  • ti5_control/msg/JointState

    • joint_id、radian、status

  • ti5_control/msg/JointStateList

    • states: JointState[]

备注

关节映射说明：

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

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

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

控制周期（来自 robot_joint_config.yaml）：

  • state_publish_period_ms：状态发布周期（默认 10ms）

  • state_query_period_ms：状态查询周期（默认 2000ms）

  • pending_command_period_ms：命令下发周期（默认 5ms）

3. 服务接口

3.1 服务列表

joint_manager 服务：

  • ti5/joint_action（ti5_control/srv/JointAction）

  • ti5/body_move（ti5_control/srv/BodyMove）

arm_ik_node 服务：

  • ti5/arm_ik（ti5_control/srv/ArmIK）

  • ti5/arm_fk（ti5_control/srv/ArmFK）

JointAction.srv（当前版本）动作常量：

  • ACTION_HOME = 0

  • ACTION_SHIFT_TO = 2（仅 T170D 机型）

  • ACTION_CLEAR_ALARM = 3

  • ACTION_MOTION_HI = 4

请求字段：

  • action_type、velocity

  • param1/param2/param3/param4（其中 ACTION_SHIFT_TO 时 param1 为高度 mm，范围 0~175；仅 T170D 机型生效）

3.2 BodyMove 字段

BodyMove.srv 关键字段：

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

  • use_neck：是否执行头部（Neck）目标；布尔开关

  • use_waist：是否执行腰部（Waist）目标；布尔开关

  • right_arm_radians[7] / left_arm_radians[7]：双臂目标角；各臂 7 个关节目标角，单位 rad

  • neck_radians[3] / waist_radians[3]：头/腰目标角；单位 rad；按型号配置分组大小取前 N 个，剩余可填 0

  • velocity：轨迹规划速度；单位 rad/s；<=0 时服务端使用默认值 0.5

3.3 ArmIK / ArmFK 字段

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

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

  • arm：手臂选择；left 或 right

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

  • target_rx/ry/rz（IK 输入）：末端姿态；rx/ry/rz 对应 roll/pitch/yaw，单位 rad

  • j2（IK 输入）：构型参数（肘关节相关）；单位 rad；当 j2 == 0 时，服务端默认：左臂 +10deg、右臂 -10deg

  • joint_angles[7]（IK 输出 / FK 输入）：7 轴关节角；单位 rad；顺序与 ti5_control/config/<TYPE>/robot_joint_config.yaml 中 Right_Arm/Left_Arm 一致

  • x/y/z/rx/ry/rz（FK 输出）：末端位姿结果；由关节角计算得到；单位/坐标系约定与 IK 输入一致

控制链路（简要）：

  1. 上层程序发布 ti5/jointcmd_list 或调用动作服务；

  2. joint_manager 按型号配置映射 joint_id -> app_id 并执行控制；

  3. 系统周期发布 ti5/jointstate_list 作为状态回读。

4. 示例程序

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

4.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}"

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

两者区别（核心）：

控制方式

处理逻辑

适用场景

BodyMove

服务端进行轨迹规划与插补（五次多项式轨迹），再连续下发关节命令

平滑动作、多部位联动

JointCmdList

直接发布关节目标，按消息内容立即生效，不经过 BodyMove 轨迹规划

单关节/单手臂快速调试

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}"

备注

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 "{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 "{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}]}"

4.3 运动学示例（FK / IK）

正解 FK（示例：把一组右臂关节角转回末端位姿）：

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

逆解 IK（示例：T170D 左臂末端到目标位姿）：

ros2 service call ti5/arm_ik ti5_control/srv/ArmIK "{robot_type: 'T170D', 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}"

j2 传 0.0 时表示使用默认构型（左臂 +10deg，右臂 -10deg）。

4.4 轨迹控制与运动学结合

推荐流程：

  1. 先分别调用左右臂 IK，得到左右臂各 7 个关节角结果：

ros2 service call ti5/arm_ik ti5_control/srv/ArmIK "{robot_type: 'T170D', 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: 'T170D', 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}"

  1. 再将左右臂 IK 返回的关节角结果填入 BodyMove，一次下发：

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}"

4.5 二次开发示例（ti5_control_client）

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

  • 可用 ./script/run_ti5_client.sh --type T140A 快速启动示例节点。

  • 也可直接运行（示例：T170D）：ros2 run ti5_control_client joint_cmd_client --ros-args -p robot_type:=T170D。注意：robot_type 必须与启动 joint_manager --type 保持一致。

  • 若修改了 ti5_control_client（或其它源码包），需在工作空间根目录重新执行 colcon build --symlink-install，并在当前终端重新 source install/setup.bash 后再运行。