参考

1. 安装流程

rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get install ros-melodic-catkin python-catkin-tools
sudo apt install ros-melodic-moveit

2.小测试前的准备

mkdir -p ~/ws_moveit/src
git clone https://github.com/ros-planning/moveit_tutorials.git -b melodic-devel
git clone https://github.com/ros-planning/panda_moveit_config.git -b melodic-devel

⚠️ For now we will use a pre-generated panda_moveit_config package but later we will learn how to make our own in the MoveIt Setup Assistant tutorial.

cd ~/ws_moveit/src
rosdep install -y –from-paths . –ignore-src –rosdistro melodic
cd ~/ws_moveit
catkin_make

source ~/ws_moveit/devel/setup.bash

3. 小测试

roslaunch panda_moveit_config demo.launch rviz_tutorial:=true

Collision-aware IK：会一直尝试寻找是否有没有Collision的做法。

4. Move Group Python Interface

在3的基础上继续，先看效果：

rosrun moveit_tutorials move_group_python_interface_tutorial.py

具体解释：

import sys
import copy
import rospy
# 提供了MoveGroupCommander,PlanningSceneInterface,RobotCommander这3个类
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

之后代码主要部分为class MoveGroupPythonIntefaceTutorial(object):
我们一个一个看它其中的函数

4.0 辅助函数 all_close(goal,actual,tolerance)

对比两个列表中的数字之差是否大于Tolerance

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

4.1 Init

 def __init__(self):
    super(MoveGroupPythonIntefaceTutorial, self).__init__()
    
    ## 首先初始化 `moveit_commander`_ 和一个名为move_group_python_interface_tutorial的`rospy`_ node:
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
    
    # 初始化一个RobotCommander的对象，它会提供机器人运动学和当前joint state的信息。
    # Rappel: RobotCommander是moveit_commander中的一个类
    robot = moveit_commander.RobotCommander()

    # 创建一个PlanningSceneInterface对象，用于改变机器人内部对于周围世界的理解。
	# Rappel: PlanningSceneInterface是moveit_commander中的一个类
    scene = moveit_commander.PlanningSceneInterface()

    # 这里我们Planning的Group是panda_arm，这个名字可以在RViz的Planning Group下拉框中看到
    # 也可以用robot.get_group_names得到
    # 用于Plan和Excute不同的动作:
    # Rappel: MoveGroupCommander是moveit_commander中的一个类
    group_name = "panda_arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)

    ## Create a `DisplayTrajectory`_ ROS publisher which is used to display
    ## trajectories in Rviz:
    # 用于在Rviz中显示路径的Publisher
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)
	
	## 得到基本信息：
    # 得到机器人的Reference Frame: 如world。
    planning_frame = move_group.get_planning_frame()
    print "============ Planning frame: %s" % planning_frame

    # end-effector link for this group名字：panda_link8
    eef_link = move_group.get_end_effector_link()
    print "============ End effector link: %s" % eef_link

    # We can get a list of all the groups in the robot:
    # 'hand','panda_arm','panda_arm_hand'
    group_names = robot.get_group_names()
    print "============ Available Planning Groups:", robot.get_group_names()

    # Sometimes for debugging it is useful to print the entire state of the robot:
    # joint_state,multi_dof_joint_state,attached_collision_objects,is_diff
    print "============ Printing robot state"
    print robot.get_current_state()
    print ""

    # Misc variables
    self.box_name = ''
    self.robot = robot
    self.scene = scene
    self.move_group = move_group
    self.display_trajectory_publisher = display_trajectory_publisher
    self.planning_frame = planning_frame
    self.eef_link = eef_link
    self.group_names = group_names

我们从最后打印的结果上知道：

1. panda一共有9个joint，7个arm，2个finger。我们这里只能更改前7个，因为选定了panda_arm。

4.2 Planning to a Joint Goal

# 让机械臂回到一个更好的位置。
 def go_to_joint_state(self):
    # 实战中应该用self.move_group
    move_group = self.move_group
    
    # 选定初始位置避免Singularity
    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

    # 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
    # 让panda_arm移到指定位置
    move_group.go(joint_goal, wait=True)

    # 使用stop避免多余动作。
    move_group.stop()

    # For testing:
    current_joints = move_group.get_current_joint_values()
    return all_close(joint_goal, current_joints, 0.01)

4.2 Planning to a Pose Goal

def go_to_pose_goal(self):
    # 实战中应该用self.move_group
    move_group = self.move_group


    # 让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

    move_group.set_pose_target(pose_goal)

    ## 对于一个Pose，在set_pose_target之后就可以直接go，无需更多参数
    plan = move_group.go(wait=True)
    # 使用stop避免多余动作。
    move_group.stop()
    
    # 执行完要及时删除Pose_targets()
    move_group.clear_pose_targets()

    # 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.move_group.get_current_pose().pose
    return all_close(pose_goal, current_pose, 0.01)

4.3 Cartesian Paths

# 用Cartesian的方法动到3个指定weight points：z-y+,x+.y-
# 用move_group.compute_cartesian_path得到一个plan和fraction
# 会显示规划的路径，但是不会改变机械臂初始位置。
def plan_cartesian_path(self, scale=1):
    # 实战中应该用self.move_group	
    move_group = self.move_group

	# 直接为end-effector指定一系列waypoints，weightpoints要是geometry_msgs.msg.Pose()
    # If executing interactively in a Python shell, set scale = 1.0. 因此默认是1。
    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))

    # We will disable the jump threshold by setting it to 0.0,
    # ignoring the check for infeasible jumps in joint space, which is sufficient
    # for this tutorial.
    (plan, fraction) = move_group.compute_cartesian_path(
                                       waypoints,   # waypoints to follow
                                       0.01,        # eef_step,1cm的resolution，慢慢移到weight points
                                       0.0)         # jump_threshold

    return plan, fraction

4.4 Displaying a Trajectory

# 利用RViz来可视化一个plan，即一个Trajectory。
# 会显示规划的路径，但是不会改变机械臂初始位置。
 def display_trajectory(self, plan):
    robot = self.robot
    display_trajectory_publisher = self.display_trajectory_publisher

    ## 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):
    
    # DisplayTrajectory msg 由 trajectory_start (当前位置) 和 trajectory(Plan) 组成。
    display_trajectory = moveit_msgs.msg.DisplayTrajectory()
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    # 利用Publish显示一个DisplayTrajectory。
    display_trajectory_publisher.publish(display_trajectory);
    

4.5 Executing a Plan

def execute_plan(self, plan):
    move_group = self.move_group
    move_group.execute(plan, wait=True)
	
	# Robot 初始位置不能偏离Plan太远

4.6 Add A Box

def add_box(self, timeout=4):
    box_name = self.box_name
    scene = self.scene

    ## First, we will create a box in the planning scene at the location of the left finger:
    # 在机器人左手手指上创建一个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
	
	# 实际上直接用self.box_name更新即可
    self.box_name=box_name
    return self.wait_for_state_update(box_is_known=True, timeout=timeout)
    # 最后这个函数是为了保证添加Box成功

4.7 确保Updates成功

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
    
    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]) # 查看scene中的attached_objects
      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()	# 查看scene中的objects

      # 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

4.8 Attach Box

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
    
    ## 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.
    # Rappel: Panda有3个Group：hand,panda_arm,panda_arm_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)

4.9 Detach from the Robot and Remove from the Planning Scene

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

    ## 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):
    box_name = self.box_name
    scene = self.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
    
    # We wait for the planning scene to update.
    return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)

4.10 总结