逆运动学固定抓取

01问题引入

前五课你集齐了所有零件：协议会发、动作会放、IK 会解。现在把它们串成一件事——抓住桌上那个立方块。视觉还没有介入（那是 EXP-07 的事），目标坐标由你用刻度尺量出来、手工发布。别小看"写死的坐标"：从坐标到成功抓起，中间隔着目标缓存、IK 反解、分步移动、六阶段状态机、下探深度一整条链，任何一环失调都会撞桌、撞臂或空抓。

02原理通俗讲解

set_pick_target 像把东西写进"待办清单"，pick 像按下"执行"键。发话题只是记下"我要抓 (0.28, 0, 0.10)"——胳膊一动不动；直到 Trigger 服务被调用，状态机才开工。这个"先记账、后执行"的设计让你可以慢慢核对目标，而不用担心手一抖臂就飞出去。

分步移动 = 挥铁锹先对准、再下铲。如果五个关节同时动，末端轨迹像泼水一样不可预测，还极易经过奇异位姿。car1 的 two_stage_move()：STAGE 1 只转底座 J1 对准目标方位，STAGE 2 保持 J1、驱动 J2-J5 到目标姿态——先粗后精，工业机械臂的标配打法。

六阶段状态机 = 一套广播体操口令。IDLE（立正）→ APPROACH（抬手到预抓点）→ DESCEND（下探）→ GRASP（合爪）→ LIFT（提起）→ HOLD（抱着待命）。每个口令只做一件事，做完检查（软限位、奇异）再进下一个——出错随时能停在明确的阶段上排查。

源码里 _execute_pick() 的六个动作，逐条对上：

03关联知识点

四组抓取参数（GraspConfig，全部米/弧度）：

四位姿四元数速查（orientation 字段）：目标垂直向下用 {x:0, y:1, z:0, w:0}（pitch=180°）；示例放置点 (0.059, 0.074, 0.219) 用 {x:0, y:0.9826, z:0, w:0.1856}。

状态机遥测：/arm/grasp_state 话题字段 current_state(string)、target_pose、progress(0-100)；状态值 IDLE、APPROACH、DESCEND、GRASP、LIFT、HOLD、PLACE_DESCEND、RELEASE。

参数调优（讲义 6.2 口径）：四参数是节点参数（运行时传入；没有"按物体形状分组的配置文件"——不同物块调出不同值是调参结果，不是预置分组）。抓之前用 预演三工具核对：① 仿真器 http://<IP>:8000/viewer（/viewer 后缀不能省）；② 离线 FK pwm_to_fk_demo.py 2150 1200 2200 2275 1500（输入五路 PWM，输出 TCP 位姿/RPY/四元数/PoseStamped）；③ 单独发 target_pose 看 5 路 PWM 确认无一路接近 500/2500 极限。

04实验目标

• 掌握 /arm_controller/set_pick_target 话题与 pick/place Trigger 服务的完整工作流程，说清"只记账"与"立即动"的区别。
• 独立完成一次端到端固定抓取：设点 → pick（5~8 秒六阶段）→ 设放置点 → place。
• 能读懂抓取日志中的 STAGE 1/STAGE 2 与 /arm/grasp_state 状态迁移。
• 调优抓取参数：以默认 0.0 为基线，做 gripper_depth 0 / 0.01 / 0.03 三档对比，并实证"改 lift_height 无效 → 该调的是 pre_grasp_height"。
• 能用软限位（servo_limits.yaml）与 Jacobian 条件数奇异检测解释"为什么抓取会中止"。

05实验环境与准备

06动画讲解：抓取六阶段 + 分步移动

动画 10 秒一循环：先底座对准（STAGE 1），再臂部两连杆带着夹爪到预抓点、下探（DESCEND 至物体上方 pre_grasp_height → 目标点）、合爪（GRASP，绿色爪闭合）、提起（LIFT 带物离桌）进入 HOLD。物体随 LIFT 抬起、随 place 落回——每一次状态迁移前都过一遍软限位与奇异检查。

07互动讲解：抓取参数配置台

选"大球"预设填入 depth 0.03 / pitch 180°；手动把 pre_grasp_height 压到 0.02 以下点"组包"，会得到 撞桌风险 警告——对应讲义"抓取过程撞桌面 → 增大 pre_grasp_height"的排查入口。右侧输出即你要写进 pick 调用/参数发布里的完整 GraspConfig。

08实操步骤

# 步骤 1 · 启动（先 bringup 再控制器，顺序即安全）
ros2 launch car1_bringup bringup.launch.py
ros2 launch car1_arm_controller arm_controller.launch.py

# 步骤 2 · 勘察（确认接口，别猜）
ros2 topic list | grep arm_controller
ros2 service list | grep arm_controller
# 预期话题：/arm_controller/target_pose、/arm_controller/set_pick_target、/arm_controller/joint_command
# 预期服务：pick、place、set_grasp_target、set_gripper、go_home、go_work、emergency_stop
# 注意：/arm_controller/pick、/place 是 std_srvs/srv/Trigger（无请求字段）

# 步骤 3 · 离线预演（不动舵机）：五路 PWM → TCP 位姿交叉核对
cd ~/ros2_ws/src/xflz_eai_car1/arm/kinematics/kinematics
python3 pwm_to_tcp.py 1500 1100 2200 1850 1500
# 输出 TCP 位置 / RPY / 四元数，用于核对 set_pick_target 的目标
# 仿真预演：http://<设备IP>:8000/viewer  （/viewer 后缀不能省）

# 步骤 4a · 设置抓取目标点（只记账，臂不会动）
ros2 topic pub --once /arm_controller/set_pick_target geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'base_link'}, pose: {position: {x: 0.28, y: 0.0, z: 0.10}, orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}}"
# orientation y=1,w=0 → pitch 180° 夹爪垂直朝下

# 步骤 4b · 触发抓取（Trigger 无请求字段，命令体留空 "{}"）
ros2 service call /arm_controller/pick std_srvs/srv/Trigger "{}"
# 约 5~8 秒：开爪 → 抬到 pre_grasp_height → 下探 → 合爪 → 抬升 → 回 post_grasp_pose
# 另一条路（立即执行用于预检）：ros2 topic pub --once /arm_controller/target_pose ...（会直接动！）
# 过程怎么看：盯 arm_controller 的终端日志（六步顺序 + IK 解的 5 路角度），没有状态话题可 echo

# 步骤 5a · 设置放置目标点（仍走 set_pick_target，同时覆盖 pick_target 与 place_target）
ros2 topic pub --once /arm_controller/set_pick_target geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'base_link'}, pose: {position: {x: 0.059, y: 0.074, z: 0.219}, orientation: {x: 0.0, y: 0.9826, z: 0.0, w: 0.1856}}}"
# 步骤 5b · 触发放置（同样 Trigger 空体）
ros2 service call /arm_controller/place std_srvs/srv/Trigger "{}"

# 步骤 5c · gripper_depth 调优对比（基线/抬高/下探三档，用 -p 覆盖节点参数）
# 重启 arm_controller 并带上新参数：
ros2 run car1_arm_controller arm_controller --ros-args -p gripper_depth:=0.02
# 对照档：-0.040（yaml 实配，抬高）/ 0.0（节点默认，停在表面）/ +0.02（往下探）
# 可调项：pre_grasp_height 0.02~0.10（撞桌加大，抬升也靠它）；grasp_pitch 保持 180 夹爪朝下
# lift_height 改了没用（源码未使用）——这一点要在报告里实测证明一次

09重点与易错点

• 成功判据：pick 后 5~8 秒完成六阶段至 HOLD；place 后回 IDLE；全程无越限告警。
• 参数调优至少做一组对照：gripper_depth 0 / 0.01 / 0.03 各抓一次（默认 0.0 即基线），记录成功率与掉落情况。
• 坐标纪律：目标物挪了 1cm，发布坐标也要跟着改——固定抓取的"固定"指的是坐标系，不是运气。

10总结与思考

练习 · 前 / 前左 / 前右三方位抓取——同一半径 0.28 m 旋到三个方位（REP-103 坐标系：x 向前 · y 向左 · z 向上；±45° 取 cos45°≈0.707 → 0.28×0.707≈0.198）。物块要用尺子摆到对应坐标，否则夹空气：

# 方位一 · 正前（基线）
ros2 topic pub --once /arm_controller/set_pick_target geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'base_link'}, pose: {position: {x: 0.28, y: 0.0, z: 0.10}, orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}}"
ros2 service call /arm_controller/pick std_srvs/srv/Trigger "{}"

# 方位二 · 前左（+y 侧，底座向左转 45°）
ros2 topic pub --once /arm_controller/set_pick_target geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'base_link'}, pose: {position: {x: 0.198, y: 0.198, z: 0.10}, orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}}"
ros2 service call /arm_controller/pick std_srvs/srv/Trigger "{}"

# 方位三 · 前右（−y 侧，底座向右转 45°）
ros2 topic pub --once /arm_controller/set_pick_target geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'base_link'}, pose: {position: {x: 0.198, y: -0.198, z: 0.10}, orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}}"
ros2 service call /arm_controller/pick std_srvs/srv/Trigger "{}"

# 纪律：每换一个方位先用 target_pose 预检可达（09 节预检纪律），确认无 PWM 贴近 500/2500 再 pick
# 对照观察：STAGE 1 底座转向与 EXP-01 里你用话题转过的 ±45° 对得上号吗？/arm/grasp_state 看迁移