Ubuntu20.04，ROS Neotic。Move It！（2）--初探机械臂控制--使用Python控制机器人

最简单的MoveIt用户界面之一是通过基于Python的Move Group Interface。这些包装器为普通用户可能需要执行的大多数操作提供功能，特别是设置关节或姿势目标，创建运动计划，移动机器人，将对象添加到环境中以及从机器人上附着/分离对象。

观看此快速YouTube视频演示，以了解Move Group Python界面的强大功能！

首先，先source一下，不然没法运行

source ~/ws_moveit/devel/setup.bash

在你的工作空间里打开一个终端

roslaunch panda_moveit_config demo.launch

现在，使用rosrun以下命令直接在另一个shell中运行Python代码：
然后重新打开一个终端

当然，还需要在source一下

source ~/ws_moveit/devel/setup.bash

rosrun moveit_tutorials move_group_python_interface_tutorial.py

这一块的完整代码在这里

执行上面这两步，按照终端的提示不断按Enter就可以执行这个demo了。
一些基本的功能演示而已，当然了，作为一个有责任的程序员，你如果认为我就把这个demo运行到这就结束了，那你就太年轻了。

下面深度解释一下，这是自己控制机械臂的第一步。

要使用Python MoveIt接口，我们将导入moveit_commander命名空间。该名称空间为我们提供了MoveGroupCommander类，PlanningSceneInterface类和RobotCommander类。这些在下面更多。我们还将导入rospy和一些我们将使用的消息：

# Python 2/3 compatibility imports
from __future__ import print_function
from six.moves import input

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
try:
  from math import pi, tau, dist, fabs, cos
except: # For Python 2 compatibility
  from math import pi, fabs, cos, sqrt
  tau = 2.0*pi
  def dist(p, q):
    return sqrt(sum((p_i - q_i)**2.0 for p_i, q_i in zip(p,q)))
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

首先初始化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_arm”。如果使用其他机器人，请将此值更改为机器人手臂计划组的名称。该界面可用于计划和执行动作：

group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)

创建一个DisplayTrajectory ROS发布器，用于在Rviz中显示轨迹：

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                               moveit_msgs.msg.DisplayTrajectory,
                                               queue_size=20)

获取基本信息

# 我们可以获得该机器人的参考框架的名称：:
planning_frame = move_group.get_planning_frame()
print("============ Planning frame: %s" % planning_frame)

# 我们也可以打印该组的末端执行器链接的名称:
eef_link = move_group.get_end_effector_link()
print("============ End effector link: %s" % eef_link)

# 我们可以获得机械手中所有组的列表：
group_names = robot.get_group_names()
print("============ Available Planning Groups:", robot.get_group_names())

# 有时，打印机器人的整个状态对于调试很有用：
print("============ Printing robot state")
print(robot.get_current_state())
print("")

计划实现共同目标

panda的zero configuration是一个奇异点，所以我们要做的第一件事是将其移动到稍微更好的配置。为了方便起见，我们使用常数tau = 2 * pi：

# 我们从组中获取关节值并更改一些值：:
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -tau/8
joint_goal[2] = 0
joint_goal[3] = -tau/4
joint_goal[4] = 0
joint_goal[5] = tau/6  # 1/6 of a turn
joint_goal[6] = 0

#如果已经为组设置了姿势或关节目标，则可以使用关节值、姿势或不使用任何参数来调用go命令
move_group.go(joint_goal, wait=True)

# 调用``stop（）``可以确保没有剩余的移动
move_group.stop()

规划姿势目标

我们可以为此组计划一个运动到最终执行者所需的姿势：

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

move_group.set_pose_target(pose_goal)

现在，我们调用计划程序来计算计划并执行它。

plan = move_group.go(wait=True)
#调用'stop（）`可确保没有剩余移动
move_group.stop()
# 在规划好姿势后，最好能明确你的目标。
#注：clear\u joint\u value\u targets（）没有等效函数
move_group.clear_pose_targets()

笛卡尔路径

您可以通过指定末端执行器要通过的航路点列表来直接规划笛卡尔路径。如果在Python Shell中交互执行，则设置scale = 1.0。

waypoints = []

wpose = move_group.get_current_pose().pose
wpose.position.z -= scale * 0.1  # First move up (z)
wpose.position.y += scale * 0.2  # and sideways (y)
waypoints.append(copy.deepcopy(wpose))

wpose.position.x += scale * 0.1  # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))

wpose.position.y -= scale * 0.1  # Third move sideways (y)
waypoints.append(copy.deepcopy(wpose))

# 我们希望笛卡尔路径以1cm的分辨率进行插值，
#这就是为什么我们将在笛卡尔平移中指定0.01作为eef\u步长的原因。
#我们将通过将跳转阈值设置为0.0来禁用跳转阈值，
#忽略对关节空间中不可行跳转的检查，这对于本教程已经足够了。
(plan, fraction) = move_group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold

# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction

显示轨迹

您可以要求RViz为您可视化计划（即轨迹）。但是group.plan（）方法会自动执行此操作，因此在这里并不是那么有用（它只是再次显示相同的轨迹）：

一个DisplayTrajectory味精有两个主要领域，trajectory_start和轨迹。我们用当前的机器人状态填充trajectory_start，以复制任何AttachedCollisionObjects并将我们的计划添加到该轨迹中。

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory);

执行计划

如果您希望机器人遵循已经计算出的计划，请使用执行：

move_group.execute(plan, wait=True)

注意：机器人的当前关节状态必须在RobotTrajectory中的第一个航路点的公差范围内，否则execute()将失败

将对象添加到计划场景

首先，我们将在规划场景中的手指之间创建一个框：

box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_hand"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.z = 0.11 # above the panda_hand frame
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.075, 0.075, 0.075))

确保收到碰撞更新

如果Python节点在发布碰撞对象更​​新消息之前死亡，则该消息可能会丢失并且该框将不会出现。为确保进行更新，我们等到看到在get_attached_objects()和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

  #测试盒子是否在现场。
#请注意，附加该框会将其从已知对象中移除
  is_known = box_name in scene.get_known_object_names()

  # 测试我们是否处于期望状态
  if (box_is_attached == is_attached) and (box_is_known == is_known):
    return True

  # sleep以便我们在处理器上给其他线程时间
  rospy.sleep(0.1)
  seconds = rospy.get_time()

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

将物体附加到机器人

接下来，我们将物体装到panda 的手上。操纵物体需要机器人能够触摸它们，而计划场景不会将接触报告为碰撞。通过将链接名称添加到touch_links数组，我们告诉计划场景忽略那些链接与框之间的碰撞。对于熊猫机器人，我们设置为。如果使用其他机器人，则应将此值更改为末端执行器组名称的名称。grasping_group = ‘hand’
注意attach_box（）函数

grasping_group = 'hand'
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)

从机器人上取下物体

我们还可以从规划场景中分离和移除对象：
注意remove_attached_object（）函数

scene.remove_attached_object(eef_link, name=box_name)

从规划场景中删除对象

我们可以将盒子从世界上移开。

scene.remove_world_object(box_name)

注意：必须先分离对象，然后才能将其从世界上移除