本人在一个机械臂项目中建模了一个四自由度机械臂,成功导入MoveIt后,机械臂可以通过api或RViz GUI进行关节空间规划(指定机械臂构型)。因为四自由度机械臂末端规划几乎无法同时指定姿态和位置,所以选用set_position_target()函数指定了end_effector的位置,进行单点规划。规划的具体语句(其中group是commander类,用法见MoveGroupCommander类):
group.set_position_target([-1.515159, 1.970219, 0.501001], 'link4')
success = group.go(wait=True)
group.stop()
group.clear_pose_targets()
但是还是没能找到逆运动学解,moveit终端报错:
arm_nc/arm_nc: Unable to sample any valid states for goal tree
arm_nc/arm_nc: Created 1 states (1 start + 0 goal)
No solution found after 5.009792 seconds
规划节点终端报错规划超时:[ INFO] [1685523788.560167099]: ABORTED: TIMED_OUT
翻了社区后,有一个有代表性的帖子:https://answers.ros.org/question/322366/moveit-unable-to-sample-any-valid-states-for-goal-tree/
总结有两个解决办法:
1、(用set_pose_target)orientation和position的精度要给到%.6f级,如pose.orientation.w=1.000000。
2、增大tolerance,通过set_position_tolerance()和set_orientation_tolerance()完成。
但这两种方法在我这都不work,最后尝试解决了,于是增加第三种解决办法,前两种不行的朋友可以试试:
在moveit规划包内的config文件夹内,修改Kinematics.yaml文件,在后面加入:
position_only_ik: True
指定逆运动学只考虑位置约束,不考虑姿态约束,加入后规划成功。这说明,即使官方文档中set_position_target()的注释为:
“Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.”
在plan过程中还是存在姿态约束。
总结:
低自由度的机械臂用moveit实现轨迹规划时,可以考虑将position_only_ik打开,以保证输入的点都有近似的逆运动学解。