python代码控制机械臂_【ROS-Moveit!】机械臂控制探索（3）——基于python的API示例代码分析...
#!/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 的零位是一个奇异点,因此我们首先将其移动到一个更好的位置上。
## （什么是奇异点？见）
# 我们从运动组（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