ROS机械臂 Movelt 学习笔记4 | Move Group 接口 Python

Python 的使用总是比 C++ 简单许多，Move Group 的 Python 接口更为便捷，也为使用者提供了很多用于操纵机器人和机械臂的函数，能够和 C++ 接口实现相同的功能：

设置机械臂的位姿
进行运动规划
移动机器人本体
将物品添加到环境 / 从环境移除
将物体绑定到机器人 / 从机器人解绑

1. 执行示例代码

1.1 运行过程

cd ~/ARM/ws_moveit/

source devel/setup.bash

# 打开机器人模型结点

roslaunch panda_moveit_config demo.launch

新开一个终端，在相同目录下执行

source devel/setup.bash

# 机器人python 接口结点

rosrun moveit_tutorials move_group_python_interface_tutorial.py

1.2 预期效果

在RViz 中，会看到以下效果：

规划并运动到指定的关节目标 joint goal
规划并运动到指定的姿态目标 pose goal
规划并显示一个笛卡尔轨迹
重演一遍 3 中的笛卡尔轨迹（执行一个保存/规划过的轨迹）
在 Panda 的末端执行器处添加一个盒子
改变盒子的颜色，表示已经被机械臂抓到了
机械臂抓着这个盒子执行一段笛卡尔轨迹
再次改变盒子颜色，表示从机械臂处解绑
移除盒子。

2. 代码分析

2.1 初始化

2.1.1 引入包

使用python接口时，我们通常需要加载moveit_commander ，这是一个namespace package，里面会加载MoveGroupCommander类, PlanningSceneInterface

以及RobotCommander类；以及其他的一些类和消息msg。

后续学习到细节时，可能会讲解这些类。但目前没有必要，因为我们学习的就是这个接口。

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

2.1.2 初始结点

初始化moveit_commander，创建一个node结点：

# 初始化moveit_commander

moveit_commander.roscpp_initialize(sys.argv)

# rospy node初始化

rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

2.1.3 创建对象

创建/实例化一个RobotCommander的对象。RobotCommander是针对整个机器人的控制，会提供机器人的运动学模型和机器人的当前关节状态：

robot = moveit_commander.RobotCommander()

创建一个PlanningSceneInterface的对象。PlanningSceneInterface是用于和机器人环境的交互（如添加物体时会用到）：

scene = moveit_commander.PlanningSceneInterface()

创建 MoveGroupCommander 的对象。 MoveGroupCommander 是针对一个规划组进行控制。这里通过设置 group_name = panda_arm 控制 Panda 机器人的手臂。

规划组的概念在C++部分以及基本概念中都提到过，是我们控制规划和执行运动的对象。

在 panda 机器人中，panda_arm 是主要手臂关节。

创建一个Publisher，发布的类型是DisplayTrajectory，用于 rivz 中显示轨迹

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

                                               														queue_size=20)

2.2 获取基本信息

# 获取机器人的参考坐标系

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 ""

2.3 Planning to a Joint Goal

规划到一个关节目标，与C++ 不同的是需要使用弧度制。

因为 panda 机器人的初始位置是一个奇点，先移动到一个较好的位置，这个动作使用 joint goal 规划：

# 赋值调整关节姿态

joint_goal = move_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()命令规划并执行动作，如果设置好了，可以不需要第一个参数

move_group.go(joint_goal, wait=True)

# 调用 stop() 确保没有多余的移动

move_group.stop()

2.4 Planning to a Pose Goal

同样的，也可以为末端执行器设置目标姿态来进行机械臂规划：

位置xyz，角度w，合起来是位姿

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

# 这个函数也与 C++ 中相同

move_group.set_pose_target(pose_goal)

调用规划器计算并执行。

plan = move_group.go(wait=True)

move_group.stop()

#完成规划后清除目标

move_group.clear_pose_targets()

注意只有pose goal这里可以有清除目标的函数clear_pose_targets()，joint value goal 没有这个对应的函数。

2.5 Cartesian Paths

笛卡尔路径。

通过指定末端执行器要通过的途径点列表，直接规划笛卡尔路径。

如果在 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的分辨率进行插值，所以设置eef为0.01

# jump_threshold跳转阈值为0.0

(plan, fraction) = move_group.compute_cartesian_path(

                                   waypoints,   # waypoints to follow

                                   0.01,        # eef_step

                                   0.0)         # jump_threshold

# 注意这里只是规划一个笛卡尔路径，但机械臂还没有执行。

return plan, fraction

2.6 Displaying a Trajectory

在 RViz 中显示路径。调用 group.plan() 规划路径的时候会自动在 rviz 中显示。

也可以手动进行显示，创建一个 DisplayTrajectory 的消息对象，然后发布到 '/move_group/display_planned_path' 话题。见下面代码：

#创建显示路径的对象

display_trajectory = moveit_msgs.msg.DisplayTrajectory()

# 用当前状态作为起始点

display_trajectory.trajectory_start = robot.get_current_state()

# plan 是传入函数的参数，是已知的

display_trajectory.trajectory.append(plan)

# Publish

display_trajectory_publisher.publish(display_trajectory);

DisplayTrajectory msg 主要有两个属性：

起始点 trajectory_start
路径 trajectory

2.7 Executing a Plan

规划完毕 plan 之后，要让机器人实际执行这个规划，就使用 move_group.execute()。

move_group.execute(plan, wait=True)

这个函数要求起始点在机器人轨迹的可允许的误差内（即符合机器人的动力运动学）

2.8 物体操作

2.8.1 添加物体

在planning scence 中机器人左指的位置添加一个 box。

box_pose = geometry_msgs.msg.PoseStamped()

box_pose.header.frame_id = "panda_leftfinger"

box_pose.pose.orientation.w = 1.0

box_pose.pose.position.z = 0.07 # slightly above the end effector

box_name = "box"

scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

注意，这里使用的坐标系是左指坐标系，这样就很容易确定 box 与左指的位置。

box_pose = geometry_msgs.msg.PoseStamped()

box_pose.header.frame_id = "panda_leftfinger"

box_pose.pose.orientation.w = 1.0

box_pose.pose.position.z = 0.07 # slightly above the end effector

box_name = "box"

scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

2.8.2 检查是否发布成功

在添加/移除物体后，会发布一个更新碰撞物体(collision object) 的消息，这个消息在发布出去的时候可能会丢失。

为了确保物体成功添加/移除，可以通过get_known_object_names()获取当前已知的物体来检查是否成功。

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_object_names 中

  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 so that we give other threads time on the processor

  rospy.sleep(0.1)

  seconds = rospy.get_time()

# If we exited the while loop without returning then we timed out

return False

2.8.3 将物体捆绑在机械臂上

接下来，我们将把 box 绑定到 panda 上。

绑定有一个条件，就是movelt 不能将机械臂的某些部位与物体的这种正常接触当作碰撞而报错。所以需要将这些部位/link 加入 touch_links，告诉规划场景 planning scene 忽略这些 links 与物体之间的碰撞

对于 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)

2.8.4 解绑物体

scene.remove_attached_object(eef_link, name=box_name)

将物体从机械臂上解除。

2.8.5 移除物体

物体必须要先解绑，然后才能移除。

scene.remove_world_object(box_name)

当然，这些操作都可以使用2.8.2 中的检验函数来确保规划器执行了我们的命令。源码中都使用了:

# 如移除物体操作的检验函数的参数应当为：

return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)

3. 总结

就语言角度来讲，python 更亲切一点，只需要明白类的相关概念就能很好的阅读源码。

源码呈现了很好的封装性，不同操作的代码写在同一类的不同的函数下，等待 main 函数来依次调用，而不用考虑彼此之间的一些关联。

3.1 规划方式

同样，python接口也在强调三种规划方式：

我个人认为是两种，笛卡尔路径应当是pose goal规划的延伸

joint value goal planning：
- 使用move_group.get_current_joint_values()函数得到当前关节状态的数组
- 修改数组的数值
- 注意此处数值是弧度制，带 pi
- 用move_group.go() 来进行规划
- 用move_group.stop()来停止运动
pose goal planning：
- 用geometry_msgs.msg.Pose()得到当前位置姿态；
- 修改角度w，坐标x、y、z
- 使用move_group.set_pose_target(pose_goal)来设置目标
- 同样使用 go() 和 stop() 规划和停止
- 这种规划多了一个可以清除目标的函数

物体的相关操作我就不再此列举了，上面的函数也挺清楚。