本文参考Moveit!官方文档。
系统:ubuntu 18.04 / 16.04
ROS:Melodic / Kinetic

概述

基于python的运动组API是最简单的MoveIt!用户接口。其中提供了用户常用的大量功能封装,例如:

  • 设置目标关节控制或笛卡尔空间位置
  • 创建运动规划
  • 移动机器人
  • 在环境中添加对象
  • 将对象与机器人连接或断开

 

下载示例功能包

我们通过官方的示例功能包来分析该API的用法。首先将两个官方的示例下载到工作空间的src目录并编译:

 

对Kinetic版本的ROS:

 

sudo apt-get install ros-kinetic-franka-description
cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/ros-planning/moveit_tutorials.git
git clone -b kinetic-devel https://github.com/ros-planning/panda_moveit_config.git
cd ~/catkin_ws/
catkin_make

 

对Melodic版本的ROS:

 

sudo apt-get install ros-melodic-franka-description
cd ~/catkin_ws/src
git clone https://github.com/ros-planning/moveit_tutorials.git -b melodic-devel
git clone https://github.com/ros-planning/panda_moveit_config.git -b melodic-devel
cd ~/catkin_ws/
catkin_make

 

分别是机器人模型、官方教程功能包和panda机械臂的配置功能包。

 

运行rviz和MoveGroup节点

ctrl+alt+T 打开两个终端,依次运行下列两行命令。第一行命令在终端中加载完毕后,再运行第二行:

 

roslaunch panda_moveit_config demo.launch
rosrun moveit_tutorials move_group_python_interface_tutorial.py

 

在第二个终端中依次敲击回车键,在rviz界面中可以看到机器人执行如下动作:

 

  1. 机器人规划并运动到关节目标位姿
  2. 机器人向目标位姿规划一条路径
  3. 机器人规划了一条笛卡尔空间的路径
  4. 机器人再次规划了一条笛卡尔空间路径
  5. 机器人执行了该笛卡尔空间路径
  6. 在Panda机械臂的末端出现了一个小箱子
  7. 小箱子改变了颜色,表示它现在已经附加在了机械臂上
  8. 机械臂附带着小箱子规划并执行了一条笛卡尔空间路径
  9. 小箱子再次改变颜色,说明它现在已经脱离了机械臂
  10. 小箱子消失

 

代码与注释

下面为 move_group_python_interface_tutorial.py 的代码,已将所有注释和打印语句翻译,可直接替换 ~/catkin_ws/src/moveit_tutorials/doc/move_group_python_interface/scripts/ 路径下的同名文件。

友情提示:

要充分理解本章的代码,除了要懂得基本的python语法和面向对象的编程结构以外,还需要有基本的机器人运动学知识,包括但不限于:

  • 理解关节空间位置与笛卡尔空间位姿的区别
  • 什么是路径规划,关节空间与笛卡尔空间路径规划的区别

 

#!/usr/bin/env python
#coding=utf-8
# 软件许可协议 (BSD License)
#
# 版权所有 (c) 2013, SRI International
# 保留所有权利。
#
# 这份授权条款,在使用者符合以下三条件的情形下,授予使用者使用及再散播本软件
# 包装原始码及二进位可执行形式的权利,无论此包装是否经改作皆然:
#  
#  * 对于本软件源代码的再散播,必须保留上述的版权宣告、此三条件表列,以及下
#    述的免责声明。
#  * 对于本套件二进位可执行形式的再散播,必须连带以文件以及/或者其他附于散播
#    包装中的媒介方式,重制上述之版权宣告、此三条件表列,以及下述的免责声明。
#  * 未获事前取得书面许可,不得使用 SRI INternational 或本软件贡献者之名称,
#    来为本软件之衍生物做任何表示支持、认可或推广、促销之行为。
#
# 本软件由版权所有者及其贡献者按照原样提供。任何明示或暗示的保证,包括但不限于对
# 适销性和适用于特定用途的暗示保证都不作承诺。在任何情况下,版权所有人及其贡献者
# 均不对任何直接、间接、附带、特殊、惩戒性或后果性损失(包括但不限于)
#
# 本软件是由版权所有者及本软件之贡献者以现状("as is")提供, 本软件包装
# 不负任何明示或默示之担保责任,包括但不限于就适售性以及特定目的的适用性为默示
# 性担保。版权所有者及本软件之贡献者,无论任何条件、 无论成因或任何责任主义、
# 无论此责任为因合约关系、无过失责任主义或因非违约之侵权(包括过失或其他原因等)
# 而起,对于任何因使用本软件包装所产生的 任何直接性、间接性、偶发性、特殊性、
# 惩罚性或任何结果的损害(包括但不限于替代商品或劳务之购用、使用损失、资料损失、
# 利益损失、业务中断等等),不负任何责任,即在该种使用已获事前告知可能会造成此类
# 损害的情形下亦然。
#
# 作者: Acorn Pooley, Mike Lautman
## BEGIN_SUB_TUTORIAL imports
##
## 为了使用Python MoveIt! 接口,我们需要导入 "moveit_commander" 命名空间。
## 这个命名空间提供了 "MoveGroupCommander"类、"PlanningSceneInterface"类
## 和 "RobotCommander"类(后文会详细讲到)。
## 
## 我们也需要导入 "rospy"模块和一些用到的消息类型:
##
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
## END_SUB_TUTORIAL
def all_close(goal, actual, tolerance):
  """
  本函数提供了一种测试方法,用以测试 actual 的值是否在 goal 对应值的公差范围内。
  @param: goal       目标参数。浮点型列表、Pose 类型或 PoseStamped 类型消息
  @param: actual     测试参数。浮点型列表、Pose 类型或 PoseStamped 类型消息
  @param: tolerance  公差范围。浮点数
  @returns: bool
  """
  all_equal = True
  if type(goal) is list:
    for index in range(len(goal)):
      if abs(actual[index] - goal[index]) > tolerance:
        return False

  elif type(goal) is geometry_msgs.msg.PoseStamped:
    return all_close(goal.pose, actual.pose, tolerance)

  elif type(goal) is geometry_msgs.msg.Pose:
    return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)

  return True
class MoveGroupPythonIntefaceTutorial(object):
  """MoveGroupPythonIntefaceTutorial"""
  def __init__(self):
    super(MoveGroupPythonIntefaceTutorial, self).__init__()

    ## BEGIN_SUB_TUTORIAL setup
    ##
    ## 首先,初始化 "moveit_commander" 和 "rospy" 节点:
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial',
                    anonymous=True)

    ## 实例化一个 "RobotCommander" 对象. 该对象是机器人与外界的接口:
    robot = moveit_commander.RobotCommander()

    ## 实例化一个 "PlanningSceneInterface" 对象。这个对象是机器人与周围环境的接口:
    scene = moveit_commander.PlanningSceneInterface()


    ## 实例化一个 "MoveGroupCommander" 对象,这是机器人关节组的接口。在本例中,
    ## 特指 panda 机械臂的关节组,因此我们将 group_name 赋值为"panda_arm"。
    ## 如果你使用的是其它的机器人,就应该把值修改为你的机器人对应的运动组。
    ## 这个接口可以用来规划和执行运动。
    group_name = "panda_arm"
    group = moveit_commander.MoveGroupCommander(group_name)

    ## 我们创建一个名为 “DisplayTrajectory” 的发布者,稍后用来发布机器人轨迹,传递给RViz用以可视化:
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)

    ## END_SUB_TUTORIAL

    ## BEGIN_SUB_TUTORIAL basic_info
    ##
    ## 获取基本信息
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^
    # 获取机器人参考坐标系的名称:
    planning_frame = group.get_planning_frame()
    print "============ Reference frame: %s" % planning_frame

    # 获取规划组末端执行器link的名称:
    eef_link = group.get_end_effector_link()
    print "============ End effector: %s" % eef_link

    # 获取机器人所有规划组的名称表:
    group_names = robot.get_group_names()
    print "============ Robot Groups:", robot.get_group_names()

    # 为了调试需要,有时需要获取机器人当前的整体状态:
    print "============ Printing robot state"
    print robot.get_current_state()
    print ""
    ## END_SUB_TUTORIAL

    # 其他变量
    self.box_name = ''
    self.robot = robot
    self.scene = scene
    self.group = group
    self.display_trajectory_publisher = display_trajectory_publisher
    self.planning_frame = planning_frame
    self.eef_link = eef_link
    self.group_names = group_names

  def go_to_joint_state(self):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    group = self.group

    ## BEGIN_SUB_TUTORIAL plan_to_joint_state
    ##
    ## 运动到关节空间中的目标位置:
    ## ^^^^^^^^^^^^^^^^^^^^^^^^
    ## Panda 的零位是一个奇异点,因此我们首先将其移动到一个更好的位置上。
    ## (什么是奇异点?见<https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>)
    # 我们从运动组(group)获取当前的关节角度,再对其进行一些调整:
    joint_goal = group.get_current_joint_values()
    joint_goal[0] = 0
    joint_goal[1] = -pi/4
    joint_goal[2] = 0
    joint_goal[3] = -pi/2
    joint_goal[4] = 0
    joint_goal[5] = pi/3
    joint_goal[6] = 0

    # go 命令可以用关节角度、空间位姿来调用,当已经为运动组设置好目标
    # 位置时,也可以不传递任何参数。
    group.go(joint_goal, wait=True)

    # 调用 stop() 命令以确认是否还有未完成的运动。
    group.stop()

    ## END_SUB_TUTORIAL

    # 测试:
    # 注意,由于本节代码不会包含在教程中,所以我们使用类变量
    # 而不是复制的状态变量。
    current_joints = self.group.get_current_joint_values()
    return all_close(joint_goal, current_joints, 0.01)

  def go_to_pose_goal(self):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    group = self.group

    ## BEGIN_SUB_TUTORIAL plan_to_pose
    ##
    ## 运动到笛卡尔空间中的目标位置:
    ## ^^^^^^^^^^^^^^^^^^^^^^^
    ## 我们能够为运动组的末端执行器指定一个笛卡尔位置并进行规划:
    pose_goal = geometry_msgs.msg.Pose()
    pose_goal.orientation.w = 1.0
    pose_goal.position.x = 0.4
    pose_goal.position.y = 0.1
    pose_goal.position.z = 0.4
    group.set_pose_target(pose_goal)

    ## 现在,我们调用规划器计算路径并执行:
    plan = group.go(wait=True)
    # 调用 stop() 命令以确认是否还有未完成的运动。
    group.stop()
    # 在规划完成后,清除目标位姿总是有益的。
    # 注意:没有任何函数与 clear_joint_value_targets() 等价。
    group.clear_pose_targets()

    ## END_SUB_TUTORIAL

    # 测试:
    # 注意,由于本节代码不会包含在教程中,所以我们使用类变量
    # 而不是复制的状态变量。
    current_pose = self.group.get_current_pose().pose
    return all_close(pose_goal, current_pose, 0.01)


  def plan_cartesian_path(self, scale=1):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    group = self.group

    ## BEGIN_SUB_TUTORIAL plan_cartesian_path
    ##
    ## 笛卡尔空间路径
    ## ^^^^^^^^^^^^^^^
    ## 你可以直接通过指定一系列的路径点,来为末端执行器规划
    ## 一个笛卡尔空间路径:
    ##
    waypoints = []

    wpose = group.get_current_pose().pose
    wpose.position.z -= scale * 0.1  # 首先向上方 (z)
    wpose.position.y += scale * 0.2  # 和侧方运动 (y)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.x += scale * 0.1  # 然后进行前/后运动 (x)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.y -= scale * 0.1  # 最后再进行侧方运动 (y)
    waypoints.append(copy.deepcopy(wpose))

    # 我们想要在1厘米的分辨率内插值笛卡尔坐标路径,因此我们在笛卡尔
    # 坐标平移中指定0.01作为步长。我们将禁用跳转阈值,设置为0.0:
    (plan, fraction) = group.compute_cartesian_path(
                                       waypoints,   # 路径点
                                       0.01,        # 步长
                                       0.0)         # 跳转阈值(jump_threshold)

    # 注意:在这里,我们只是进行规划,还没有用 move_group 来实际驱动机器人。
    return plan, fraction

    ## END_SUB_TUTORIAL

  def display_trajectory(self, plan):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    robot = self.robot
    display_trajectory_publisher = self.display_trajectory_publisher

    ## BEGIN_SUB_TUTORIAL display_trajectory
    ##
    ## 显示一条轨迹
    ## ^^^^^^^^^^^^^^^^^^^^^^^
    ## 你可以使用RViz来进行规划(又名轨迹)的可视化。但这里的 
    ## group.plan() 方法可以自动进行可视化,因此就没有必要了。
    ##
    ## "DisplayTrajectory" 消息有两个主要的值:trajectory_start 和 trajectory。
    ## 我们使用当前机器人状态填充trajectory_start,以便复制任何 AttachedCollisionObjects,
    ## 并将我们的轨迹添加到trajectory中。
    display_trajectory = moveit_msgs.msg.DisplayTrajectory()
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    # 发布消息
    display_trajectory_publisher.publish(display_trajectory)

    ## END_SUB_TUTORIAL

  def execute_plan(self, plan):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    group = self.group

    ## BEGIN_SUB_TUTORIAL execute_plan
    ##
    ## 执行一个规划
    ## ^^^^^^^^^^^^^^^^
    ## 如果想要机器人根据已经规划好的路径运动,使用 execute 函数:
    group.execute(plan, wait=True)

    ## **注意:** 机器人当前的关节位置必须在 RobotTrajectory 的
    ## 第一个路径点的公差范围内,否则 execute() 函数将会失败。
    ## END_SUB_TUTORIAL

  def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    box_name = self.box_name
    scene = self.scene

    ## BEGIN_SUB_TUTORIAL wait_for_scene_update
    ##
    ## 确定已经收到碰撞更新
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## 如果Python节点在发布碰撞对象更新消息之前完结(die),则消息
    ## 可能丢失,箱子将不会出现。为了确保进行了更新,我们要等到看到 
    ## "get_known_object_names()" 和 "get_known_object_names()" 
    ## 列表中反映的更改。出于本教程的目的,我们在规划场景中添加、删除、
    ## 附加或分离对象后调用此函数。然后,我们等待更新完成或 “timeout” 秒过去。
    start = rospy.get_time()
    seconds = rospy.get_time()
    while (seconds - start < timeout) and not rospy.is_shutdown():
      # 测试箱子是否已经附着到对象上。
      attached_objects = scene.get_attached_objects([box_name])
      is_attached = len(attached_objects.keys()) > 0

      # 测试箱子是否在场景中。
      # 注意:附着箱子将会把它从 known_objects 移除。
      is_known = box_name in scene.get_known_object_names()

      # 测试是否已经达到期望状态
      if (box_is_attached == is_attached) and (box_is_known == is_known):
        return True

      # 休眠,留时间给处理器上的其他线程
      rospy.sleep(0.1)
      seconds = rospy.get_time()

    # 如果我们退出while循环而不返回,那么我们就超时了
    return False
    ## END_SUB_TUTORIAL

  def add_box(self, timeout=4):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    box_name = self.box_name
    scene = self.scene

    ## BEGIN_SUB_TUTORIAL add_box
    ##
    ## 添加对象到规划场景中
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## 首先,我们在规划场景的左边手指处创建一个箱子:
    box_pose = geometry_msgs.msg.PoseStamped()
    box_pose.header.frame_id = "panda_leftfinger"
    box_pose.pose.orientation.w = 1.0
    box_name = "box"
    scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

    ## END_SUB_TUTORIAL
    # 这里将本地变量复制回类变量。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    self.box_name=box_name
    return self.wait_for_state_update(box_is_known=True, timeout=timeout)


  def attach_box(self, timeout=4):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    box_name = self.box_name
    robot = self.robot
    scene = self.scene
    eef_link = self.eef_link
    group_names = self.group_names

    ## BEGIN_SUB_TUTORIAL attach_object
    ##
    ## 把对象附着到机器人上:
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## 接下来,我们将把箱子附着在 Panda 的手腕上。先操纵物体,直到机器人能够触摸到物体,
    ## 而规划场景不将接触报告为碰撞即可。通过向 "touch_links" 数组中添加链接名称,我们
    ## 告诉规划场景忽略这些 link 和箱子之间的碰撞。对于 Panda 机器人,我们设置 "grasping_group = 'hand'"。
    ## 如果您使用的是另一个机器人,您应该将此值更改为您的末端执行器组名称。
    grasping_group = 'hand'
    touch_links = robot.get_link_names(group=grasping_group)
    scene.attach_box(eef_link, box_name, touch_links=touch_links)
    ## END_SUB_TUTORIAL

    # 等待规划场景的更新
    return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout)

  def detach_box(self, timeout=4):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    box_name = self.box_name
    scene = self.scene
    eef_link = self.eef_link

    ## BEGIN_SUB_TUTORIAL detach_object
    ##
    ## 从机器人上解除对象
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## 我们也能够从规划场景中解除并移除对象:
    scene.remove_attached_object(eef_link, name=box_name)
    ## END_SUB_TUTORIAL

    # 等待规划场景的更新
    return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout)

  def remove_box(self, timeout=4):
    # 这里将类变量复制到本地变量,可以让本教程显得更加清晰。
    # 但在实践中,除非有充分的理由,否则应该直接使用类变量。
    box_name = self.box_name
    scene = self.scene

    ## BEGIN_SUB_TUTORIAL remove_object
    ##
    ## 从规划场景中移除对象
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    ## 我们从世界中移除箱子:
    scene.remove_world_object(box_name)

    ## **注意:** 要移除对象,对象首先应当被解除。
    ## END_SUB_TUTORIAL

    # 等待规划场景的更新
    return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)
def main():
  try:
    print "============ 按 `Enter` 开始设置 moveit_commander (按 ctrl-d 退出) ..."
    raw_input()
    tutorial = MoveGroupPythonIntefaceTutorial()

    print "============ 按 `Enter` 移动到一个关节空间的目标位置 ..."
    raw_input()
    tutorial.go_to_joint_state()

    print "============ 按 `Enter` 移动到一个笛卡尔空间的目标位置 ..."
    raw_input()
    tutorial.go_to_pose_goal()

    print "============ 按 `Enter` 规划并演示一个笛卡尔空间路径 ..."
    raw_input()
    cartesian_plan, fraction = tutorial.plan_cartesian_path()

    print "============ 按 `Enter` 演示一个存储好的路径(这将重新演示一遍笛卡尔路径) ..."
    raw_input()
    tutorial.display_trajectory(cartesian_plan)

    print "============ 按 `Enter` 执行存储好的路径 ..."
    raw_input()
    tutorial.execute_plan(cartesian_plan)

    print "============ 按 `Enter` 向规划场景中添加一个箱子(未附着状态的箱子为绿色) ..."
    raw_input()
    tutorial.add_box()

    print "============ 按 `Enter` 将箱子附着在 Panda 机器人上(附着状态的箱子为紫色) ..."
    raw_input()
    tutorial.attach_box()

    print "============ 按 `Enter` 使机器人附带着箱子规划并执行一个路径 ..."
    raw_input()
    cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)
    tutorial.execute_plan(cartesian_plan)

    print "============ 按 `Enter` 从 Panda 机器人上解除箱子(未附着状态的箱子为绿色) ..."
    raw_input()
    tutorial.detach_box()

    print "============ 按 `Enter` 从规划场景中移除箱子 ..."
    raw_input()
    tutorial.remove_box()

    print "============ Python 教程演示结束!"
  except rospy.ROSInterruptException:
    return
  except KeyboardInterrupt:
    return
if __name__ == '__main__':
  main()
## BEGIN_TUTORIAL
## .. _moveit_commander:
##    http://docs.ros.org/melodic/api/moveit_commander/html/namespacemoveit__commander.html
##
## .. _MoveGroupCommander:
##    http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html
##
## .. _RobotCommander:
##    http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1robot_1_1RobotCommander.html
##
## .. _PlanningSceneInterface:
##    http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html
##
## .. _DisplayTrajectory:
##    http://docs.ros.org/melodic/api/moveit_msgs/html/msg/DisplayTrajectory.html
##
## .. _RobotTrajectory:
##    http://docs.ros.org/melodic/api/moveit_msgs/html/msg/RobotTrajectory.html
##
## .. _rospy:
##    http://docs.ros.org/melodic/api/rospy/html/
## CALL_SUB_TUTORIAL imports
## CALL_SUB_TUTORIAL setup
## CALL_SUB_TUTORIAL basic_info
## CALL_SUB_TUTORIAL plan_to_joint_state
## CALL_SUB_TUTORIAL plan_to_pose
## CALL_SUB_TUTORIAL plan_cartesian_path
## CALL_SUB_TUTORIAL display_trajectory
## CALL_SUB_TUTORIAL execute_plan
## CALL_SUB_TUTORIAL add_box
## CALL_SUB_TUTORIAL wait_for_scene_update
## CALL_SUB_TUTORIAL attach_object
## CALL_SUB_TUTORIAL detach_object
## CALL_SUB_TUTORIAL remove_object

 

上一篇文章的遗留问题:公差检测函数 all_close()决定动作是否执行

上一篇文章的最后,我提到了rviz规划界面的不稳定性。这是因为函数all_close()多次用来检测规划状态与当前状态是否相符。在纯粹做运动规划的理想状态下,即使公差很小也不会有什么影响。但实际环境或仿真环境会影响机器人的当前状态。例如由于基座运动、刚度过小导致的位置误差,会使得公差检测不通过,进而导致运动规划失败。这种情况下,应当适当加大允许的公差范围。

 

值得研究的一个问题

在实验过程中,发现机械臂位姿偶尔会发生跳变,应该是求逆解时的多解造成的。难道 Moveit! 默认求解器无法处理多解情况?这个问题值得研究一下。

 

演示效果

[video width="848" height="476" mp4="https://www.guyuehome.com/Uploads/wp/2020/05/v3_195549ac-669f-11e9-8353-0a580a45ec2c.mp4"][/video]

ROS机械臂的控制API示

 

相关课程:

《MoveIt编程入门 · 古月》

《MoveIt可视化配置及仿真指南 · 古月》

 

参考文献

[1] Move Group Python Interface