【MTC拾取放置示例】将Connect中的最大目标偏差检查增加到1e-2,实现move to pick/move to place

问题描述

在运行Moveit2使用MTC构建拾取放置示例Pick and Place with MoveIt Task Constructor的时候出现报错

move to pick规划失败

“The computed trajectory is too short to detect jumps in joint-space. Need at least 10 steps, only got 2. Try a lower max_step”

[moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1725965360.095137621] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1] [WARN] [1725965360.151262025] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[mtc_tutorial-1]   0  - ←   0 →   -  0 / demo task
[mtc_tutorial-1]     1  - ←   1 →   -  0 / current
[mtc_tutorial-1]     -  0 →   1 →   -  1 / open hand
[mtc_tutorial-1]     -  1 →   0 ←  88  - / move to pick
[mtc_tutorial-1]    88  - ←  88 →   - 88 / pick object
[mtc_tutorial-1]      89  - ←  89 ←   7  - / approach object
[mtc_tutorial-1]       7  - ← 108 →   -  7 / grasp pose IK
[mtc_tutorial-1]        25  - ←  25 →   - 25 / generate grasp pose
[mtc_tutorial-1]       -  7 → 101 →   -  0 / allow collision (hand,object)
[mtc_tutorial-1]       -  0 → 101 →   -  0 / close hand
[mtc_tutorial-1]       -  0 → 101 →   -  0 / attach object
[mtc_tutorial-1]       -  0 →  93 →   - 93 / lift object
[mtc_tutorial-1]     - 88 →   0 ← 371  - / move to place
[mtc_tutorial-1]   371  - ← 371 →   -  0 / place object
[mtc_tutorial-1]     1408  - ← 1408 →   -  0 / place pose IK
[mtc_tutorial-1]       1010  - ← 1010 →   -1010 / generate place pose
[mtc_tutorial-1]       -  0 → 1408 →   -  0 / open hand
[mtc_tutorial-1]       -  0 → 1408 →   -  0 / forbid collision (hand,object)
[mtc_tutorial-1]       -  0 → 1408 →   -  0 / detach object
[mtc_tutorial-1]       -  0 → 371 →   -371 / retreat
[mtc_tutorial-1]     -  0 → 368 →   -368 / return home
[mtc_tutorial-1] Failing stage(s):
[mtc_tutorial-1] move to pick (0/88): Trajectory end-point deviates too much from goal state
[mtc_tutorial-1] [ERROR] [1725965360.213938990] [mtc_tutorial]: Task planning failed
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[mtc_tutorial-1] [INFO] [1725965492.316480680] [rclcpp]: signal_handler(signum=2)
[INFO] [mtc_tutorial-1]: process has finished cleanly [pid 135443]

解决方案

将Connect中的最大目标偏差检查增加到1e-2

成功运行

1. 向前传播（→）
   当一个阶段成功生成了解决方案，且这个解决方案能够传递到下一个阶段时，箭头会指向前方，表示向前传播。
   箭头向右（→） 表示解决方案从当前阶段传递到了下一阶段。
2. 向后传播（←）
   某些阶段不仅会向前生成和传递解决方案，还会回溯前面的阶段，这就是向后传播。
   箭头向左（←） 表示当前阶段的解决方案向前一个阶段传播。