ROS从入门到放弃——安装Moveit及其使用

• 1. 安装流程
• 2.小测试前的准备
• 3. 小测试
• 4. Move Group Python Interface
• • 4.0 辅助函数 all_close(goal,actual,tolerance)
  • 4.1 Init
  • 4.2 Planning to a Joint Goal
  • 4.2 Planning to a Pose Goal
  • 4.3 Cartesian Paths
  • 4.4 Displaying a Trajectory
  • 4.5 Executing a Plan
  • 4.6 Add A Box
  • 4.7 确保Updates成功
  • 4.8 Attach Box
  • 4.9 Detach from the Robot and Remove from the Planning Scene
  • 4.10 总结

参考

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 = Trueif type(goal) is list:for index in range(len(goal)):if abs(actual[index] - goal[index]) > tolerance:return Falseelif 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中显示路径的Publisherdisplay_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_link8eef_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_diffprint "============ Printing robot state"print robot.get_current_state()print ""# Misc variablesself.box_name = ''self.robot = robotself.scene = sceneself.move_group = move_groupself.display_trajectory_publisher = display_trajectory_publisherself.planning_frame = planning_frameself.eef_link = eef_linkself.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_groupmove_group = self.move_group# 选定初始位置避免Singularityjoint_goal = move_group.get_current_joint_values()joint_goal[0] = 0joint_goal[1] = -pi/4joint_goal[2] = 0joint_goal[3] = -pi/2joint_goal[4] = 0joint_goal[5] = pi/3joint_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_groupmove_group = self.move_group# 让end-effector达到指定位置pose_goal = geometry_msgs.msg.Pose()pose_goal.orientation.w = 1.0pose_goal.position.x = 0.4pose_goal.position.y = 0.1pose_goal.position.z = 0.4move_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 variablecurrent_pose = self.move_group.get_current_pose().posereturn 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().posewpose.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 follow0.01,        # eef_step,1cm的resolution，慢慢移到weight points0.0)         # jump_thresholdreturn plan, fraction

4.4 Displaying a Trajectory

# 利用RViz来可视化一个plan，即一个Trajectory。
# 会显示规划的路径，但是不会改变机械臂初始位置。def display_trajectory(self, plan):robot = self.robotdisplay_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_groupmove_group.execute(plan, wait=True)# Robot 初始位置不能偏离Plan太远

4.6 Add A Box

def add_box(self, timeout=4):box_name = self.box_namescene = self.scene## First, we will create a box in the planning scene at the location of the left finger:# 在机器人左手手指上创建一个Boxbox_pose = geometry_msgs.msg.PoseStamped()box_pose.header.frame_id = "panda_leftfinger"box_pose.pose.orientation.w = 1.0box_pose.pose.position.z = 0.07 # slightly above the end effectorbox_name = "box"scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1)) # 让机器人感知到这个box# 实际上直接用self.box_name更新即可self.box_name=box_namereturn 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_namescene = self.scenestart = rospy.get_time()seconds = rospy.get_time()while (seconds - start < timeout) and not rospy.is_shutdown():# Test if the box is in attached objectsattached_objects = scene.get_attached_objects([box_name]) # 查看scene中的attached_objectsis_attached = len(attached_objects.keys()) > 0# Test if the box is in the scene.# Note that attaching the box will remove it from known_objectsis_known = box_name in scene.get_known_object_names()	# 查看scene中的objects# Test if we are in the expected stateif (box_is_attached == is_attached) and (box_is_known == is_known):return True# Sleep so that we give other threads time on the processorrospy.sleep(0.1)seconds = rospy.get_time()# If we exited the while loop without returning then we timed outreturn False## END_SUB_TUTORIAL

4.8 Attach Box

def attach_box(self, timeout=4):box_name = self.box_namerobot = self.robotscene = self.sceneeef_link = self.eef_linkgroup_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_handgrasping_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_namescene = self.sceneeef_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_namescene = 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 总结