## 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
复制代码

## 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)
复制代码

## 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)
复制代码

## 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
复制代码

## 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
复制代码

## 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
复制代码

## 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
复制代码

## 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)
复制代码

## 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)
复制代码

## 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)
复制代码

## 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)
复制代码

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!"
复制代码