ROS学习（杂）move_group_python_interface_tutorial代码注释

#!/usr/bin/env python

# Software License Agreement (BSD License)
#
# Copyright (c) 2013, SRI International
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of SRI International nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Acorn Pooley, Mike Lautman

## BEGIN_SUB_TUTORIAL imports
##
## To use the Python MoveIt! interfaces, we will import the `moveit_commander`_ namespace.
## This namespace provides us with a `MoveGroupCommander`_ class, a `PlanningSceneInterface`_ class,
## and a `RobotCommander`_ class. (More on these below)
##
## We also import `rospy`_ and some messages that we will use:
##
##使用Python MoveIt！接口，我们将导入moveit_commander命名空间。该命名空间为我们提供了MoveGroupCommander类，
##PlanningSceneInterface类和RobotCommander类。
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):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@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
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
##首先初始化moveit_commander和一个rospy节点：
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)

## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
##实例化RobotCommander对象。该对象是机器人的外层接口：
robot = moveit_commander.RobotCommander()

## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
##实例化PlanningSceneInterface对象。该对象是机器人周围世界的接口：
scene = moveit_commander.PlanningSceneInterface()

## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints. In this case the group is the joints in the Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
##实例化MoveGroupCommander对象。该对象是一组关节的接口。在这种情况下，该组是熊猫臂的关节，所以我们设置。
## 如果您使用的是其他机器人，则应将此值更改为机器人手臂计划组的名称。此界面可用于计划和执行熊猫的动作：group_name = panda_arm
group_name = "panda_arm"
group = moveit_commander.MoveGroupCommander(group_name)

## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
##我们创建了一个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
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
#我们可以得到这个机器人的参考框架的名称：

planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame

# We can also print the name of the end-effector link for this group:
#我们还可以打印该组的末端效应器链接的名称：

eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link

# We can get a list of all the groups in the robot:
#我们可以获得机器人中所有组的列表：
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()

# Sometimes for debugging it is useful to print the entire state of the
# robot:
#有时为了调试，打印#robot的整个状态非常有用

print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL

# Misc variables
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):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group

## BEGIN_SUB_TUTORIAL plan_to_joint_state
##
## Planning to a Joint Goal
## ^^^^^^^^^^^^^^^^^^^^^^^^
## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
## thing we want to do is move it to a slightly better configuration.
##Panda的零配置是一个奇点，所以我们要做的第一件事就是将它转移到略好的配置。

# We can get the joint values from the group and adjust some of the values:
#我们可以从组中获取联合值并调整一些值：
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

# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group

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

# Calling ``stop()`` ensures that there is no residual movement
#调用``stop（）``确保没有残余运动
group.stop()

## END_SUB_TUTORIAL

# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_joints = self.group.get_current_joint_values()
return all_close(joint_goal, current_joints, 0.01)

def go_to_pose_goal(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group

## BEGIN_SUB_TUTORIAL plan_to_pose
##
## Planning to a Pose Goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector:
#我们可以为这个组计划一个动作，使其达到最终效应器的所需姿势：
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)

## Now, we call the planner to compute the plan and execute it.
#现在，我们调用计划程序来计算计划并执行它。
plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()

## END_SUB_TUTORIAL

# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_pose = self.group.get_current_pose().pose
return all_close(pose_goal, current_pose, 0.01)

def plan_cartesian_path(self, scale=1):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group

## BEGIN_SUB_TUTORIAL plan_cartesian_path
##
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a Cartesian path directly by specifying a list of waypoints
## for the end-effector to go through:
##您可以通过指定末端效应器的航点列表来直接规划笛卡尔路径：
waypoints = []
##我们希望以1 cm的分辨率插入笛卡尔路径

wpose = 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))

# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation. We will disable the jump threshold by setting it to 0.0 disabling:
#我们希望以1 cm的分辨率插入笛卡尔路径
(plan, fraction) = 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

## END_SUB_TUTORIAL

def display_trajectory(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
robot = self.robot
display_trajectory_publisher = self.display_trajectory_publisher

## BEGIN_SUB_TUTORIAL display_trajectory
##
## Displaying a Trajectory
## ^^^^^^^^^^^^^^^^^^^^^^^
## You can ask RViz to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again):
##
## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory.
## We populate the trajectory_start with our current robot state to copy over
## any AttachedCollisionObjects and add our plan to the trajectory.
#你可以让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);

## END_SUB_TUTORIAL
#如果您希望机器人遵循已计算的计划，请使用execute：
def execute_plan(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group

## BEGIN_SUB_TUTORIAL execute_plan
##
## Executing a Plan
## ^^^^^^^^^^^^^^^^
## Use execute if you would like the robot to follow
## the plan that has already been computed:
group.execute(plan, wait=True)

## **Note:** The robot's current joint state must be within some tolerance of the
## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail
## END_SUB_TUTORIAL
#注意：机器人当前的关节状态必须在RobotTrajectory中第一个航路点的某个容差范围内，否则execute()将失败

def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene

## BEGIN_SUB_TUTORIAL wait_for_scene_update
##
## Ensuring Collision Updates Are Receieved
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## If the Python node dies before publishing a collision object update message, the message
## could get lost and the box will not appear. To ensure that the updates are
## made, we wait until we see the changes reflected in the
## ``get_known_object_names()`` and ``get_known_object_names()`` lists.
## For the purpose of this tutorial, we call this function after adding,
## removing, attaching or detaching an object in the planning scene. We then wait
## until the updates have been made or ``timeout`` seconds have passed
   # 如果Python节点在发布冲突对象更新消息之前死亡，
   # 则消息可能会丢失并且不会出现该框。为了确保更新，
   # 我们要等到我们看到get_known_object_names()和get_known_object_names()列表中反映的更改
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# Test if the box is in attached objects
attached_objects = scene.get_attached_objects([box_name])
is_attached = len(attached_objects.keys()) > 0

# Test if the box is in the scene.
# Note that attaching the box will remove it from known_objects
is_known = box_name in scene.get_known_object_names()

# Test if we are in the expected state
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
## END_SUB_TUTORIAL

def add_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene

## BEGIN_SUB_TUTORIAL add_box
##
## Adding Objects to the Planning Scene
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will create a box in the planning scene at the location of the left finger:
#首先，我们将在左手指位置的规划场景中创建一个框：

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
# Copy local variables back to class variables. In practice, you should use the class
# variables directly unless you have a good reason not to.
self.box_name=box_name
return self.wait_for_state_update(box_is_known=True, timeout=timeout)

def attach_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
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
##
## Attaching Objects to the Robot
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## Next, we will attach the box to the Panda wrist. Manipulating objects requires the
## robot be able to touch them without the planning scene reporting the contact as a
## collision. By adding link names to the ``touch_links`` array, we are telling the
## planning scene to ignore collisions between those links and the box. For the Panda
## robot, we set ``grasping_group = 'hand'``. If you are using a different robot,
## you should change this value to the name of your end effector group name.
   # 接下来，我们将把盒子贴在熊猫手腕上。操纵物体需要机器人能够触摸它们，而计划场景不会将接触报告为碰撞。通过向touch_links数组添加链接名
   # 我们告诉规划场景忽略这些链接和框之间的冲突。对于熊猫机器人，我们设置。如果您使用的是其他机器人，
   # 则应将此值更改为末端效应器组名称。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

# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout)

def detach_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
eef_link = self.eef_link

## BEGIN_SUB_TUTORIAL detach_object
##
## Detaching Objects from the Robot
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## We can also detach and remove the object from the planning scene:
##我们还可以从规划场景中分离和删除对象：
scene.remove_attached_object(eef_link, name=box_name)
## END_SUB_TUTORIAL

# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout)

def remove_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene

## BEGIN_SUB_TUTORIAL remove_object
##
## Removing Objects from the Planning Scene
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## We can remove the box from the world.
##我们可以从世界上删除这个盒子。
scene.remove_world_object(box_name)

## **Note:** The object must be detached before we can remove it from the world
## END_SUB_TUTORIAL
##注意：在我们将对象从世界中删除之前，必须先分离对象

# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)

def main():
try:
print "============ Press `Enter` to begin the tutorial by setting up the moveit_commander (press ctrl-d to exit) ..."
raw_input()
tutorial = MoveGroupPythonIntefaceTutorial()

print "============ Press `Enter` to execute a movement using a joint state goal ..."
raw_input()
tutorial.go_to_joint_state()

print "============ Press `Enter` to execute a movement using a pose goal ..."
raw_input()
tutorial.go_to_pose_goal()

print "============ Press `Enter` to plan and display a Cartesian path ..."
raw_input()
cartesian_plan, fraction = tutorial.plan_cartesian_path()

print "============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ..."
raw_input()
tutorial.display_trajectory(cartesian_plan)

print "============ Press `Enter` to execute a saved path ..."
raw_input()
tutorial.execute_plan(cartesian_plan)

print "============ Press `Enter` to add a box to the planning scene ..."
raw_input()
tutorial.add_box()

print "============ Press `Enter` to attach a Box to the Panda robot ..."
raw_input()
tutorial.attach_box()

print "============ Press `Enter` to plan and execute a path with an attached collision object ..."
raw_input()
cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)
tutorial.execute_plan(cartesian_plan)

print "============ Press `Enter` to detach the box from the Panda robot ..."
raw_input()
tutorial.detach_box()

print "============ Press `Enter` to remove the box from the planning scene ..."
raw_input()
tutorial.remove_box()

print "============ Python tutorial demo complete!"
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return

if __name__ == '__main__':
main()

## BEGIN_TUTORIAL
## .. _moveit_commander:
## http://docs.ros.org/kinetic/api/moveit_commander/html/namespacemoveit__commander.html
##
## .. _MoveGroupCommander:
## http://docs.ros.org/kinetic/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html
##
## .. _RobotCommander:
## http://docs.ros.org/kinetic/api/moveit_commander/html/classmoveit__commander_1_1robot_1_1RobotCommander.html
##
## .. _PlanningSceneInterface:
## http://docs.ros.org/kinetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html
##
## .. _DisplayTrajectory:
## http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/DisplayTrajectory.html
##
## .. _RobotTrajectory:
## http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/RobotTrajectory.html
##
## .. _rospy:
## http://docs.ros.org/kinetic/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
## END_TUTORIAL

ROS学习（杂）move_group_python_interface_tutorial代码注释

猜你喜欢