机器人抓取（一）—— ROS MoveIt! 程序控制真实UR5机器人运动（python）-机器人抓取物品

moveit! ur5 机器人控制

上文在 ubuntu 系统中配置了ur 机器人ros功能包 universal_robot 和驱动 Universal_Robots_ROS_Driver，并实现了用 moveit_rviz 拖动机器人运动。本文旨在学习如何使用 moveit python 用户接口moveit_commander，实现真实的ur5 机器人轨迹规划与控制。

创建功能包

cd ur5/src # ur5是工作空间

catkin_create_pkg ur_move_test std_msgs rospy编写 python 程序

参考：MoveIt » Tutorials » Move Group Python Interface

在 ur_move_test 文件夹下新建scripts文件夹，用于存放 python 文件。在 scripts 文件夹中创建 ur_move_test_node.py 。此程序仅作为moveit路径规划与执行功能的简单示例。程序如下:

!/usr/bin/env pythonuse moveit_commander (the Python MoveIt user interfaces )

import sys

import copy

import rospy

import moveit_commander

import moveit_msgs.msg

import geometry_msgs.msg

from math import pi

class MoveGroupInteface(object):

def init

(self):

super(MoveGroupInteface, self).init() ######################### setup ############################ moveit_commander.roscpp_initialize(sys.argv) rospy.init_node(ur_move_test_node, anonymous=True)self.robot = moveit_commander.RobotCommander()self.scene = moveit_commander.PlanningSceneInterface()# Not used in this tutorial group_name =“manipulator”# group_name can be find in ur5_moveit_config/config/ur5.srdfself.move_group_commander = moveit_commander.MoveGroupCommander(group_name)self.display_trajectory_publisher = rospy.Publisher(/move_group/display_planned_path,moveit_msgs.msg.DisplayTrajectory,queue_size=20)################ Getting Basic Information ######################self.planning_frame =self.move_group_commander.get_planning_frame()print“============ Planning frame: %s”%self.planning_frameself.eef_link =self.move_group_commander.get_end_effector_link()print“============ End effector link: %s”%self.eef_linkself.group_names =self.robot.get_group_names()print“============ Available Planning Groups:”,self.robot.get_group_names()print“============ Printing robot state:”printself.robot.get_current_state()# getprint“”def plan_cartesian_path(self, scale=1): waypoints =[] wpose =self.move_group_commander.get_current_pose().pose wpose.position.z -= scale *0.1# First move up (z) 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,# ignoring the check for infeasible jumps in joint space, which is sufficient# for this tutorial.(plan, fraction)=self.move_group_commander.compute_cartesian_path( waypoints,# waypoints to follow0.01,# eef_step 0.0)# jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: pring “=========== Planning completed, Cartesian path is saved=============”return plan, fractiondef execute_plan(self, plan):## Use execute if you would like the robot to follow## the plan that has already been computed:self.move_group_commander.execute(plan, wait=True)

print “—————————————————————————————“

print “Welcome to the MoveIt MoveGroup Python Interface Tutorial”

print “—————————————————————————————“

print “Press Ctrl-D to exit at any time”

print “”

print “============ Press Enter to begin the tutorial by setting up the moveit_commander …”

raw_input()

tutorial = MoveGroupInteface()

print “============ Press Enter to plan and display a Cartesian path …”

raw_input()

cartesian_plan, fraction = tutorial.plan_cartesian_path()

print “============ Press Enter to execute a saved path …”

raw_input()

tutorial.execute_plan(cartesian_plan)

print “============ Press Enter to go back …”

raw_input()

cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)

tutorial.execute_plan(cartesian_plan)

注：上述程序中，给定路径点计算轨迹时的返回值 fraction 表示，规划的轨迹在给定路径点列表的覆盖率 [0~1]，如果fraction小于1,说明给定的路径点无法完整规划。

创建 launch文件

启动机器人控制节点，要在不同终端分别启动 ur5_bringup.launch、ur5_moveit_planning_execution.launch、moveit_rviz.launch（可选）、ur_move_test_node.py 。非常麻烦，因此编写launch文件，一次性启动上述节点。

在ur_move_test 功能包中创建launch文件夹，用以存放launch文件。

ur5机器人的启动文件: ur5_move_test.launch

<launch><!– robot_ip: IP–address of the robots socket-messaging server –> <arg name=”robot_ip” default=”192.168.1.2″ doc=”IP of the controller”/> <arg name=”kinematics_config” default=”$(find ur_description)/config/ur5_calibration.yaml”/> <arg name=”limited” default=”true” doc=”If true, limits joint range [-PI, PI] on all joints.” /> <include file=”$(find ur_robot_driver)/launch/ur5_bringup.launch”> <arg name=”robot_ip” value=”$(arg robot_ip)”/> <arg name=”kinematics_config” value=”$(arg kinematics_config)”/> </include> <include file=”$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch”> <arg name=”limited” value=”$(arg limited)”/> </include> <include file=”$(find ur5_moveit_config)/launch/moveit_rviz.launch”> <arg name=”config” value=”true”/> </include> <node name=”ur_move_test_node” pkg=”ur_move_test” type=”ur_move_test_node.py” respawn=”true” output=”screen” /></launch>

4.修改 CmakeLists.txt文件, 编译功能包

在ur_move_test 功能包的 CmakeLists.txt文件夹中找到 catkin_install_python函数，解除此部分注释，将代码修改为： Mark executable scripts (Python etc.) for installationin contrast to setup.py, you can choose the destination

catkin_install_python(PROGRAMS

scripts/ur_move_test_node.py

DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

编译功能包：

cd ur5

catkin_make # 或仅编译单个功能包 catkin_make -DCATKIN_WHITELIST_PACKAGES=“ur_move_test”

运行程序

运行之前，要给ur_move_test_node.py文件增加可执行权限：

cd ur5/src/ur_move_test/scripts

chmod +x ur_move_test_node.py

运行lunch文件

roslaunch ur_move_test ur5_move_test.launch

注意：不要忘记启动机器人示教器中的external_control.urp程序，详情参考：ubuntu18.04 ros-melodic 安装 ur_robot_driver，驱动真实的 ur5 机器人。

修改 ee_link 固定位置

ur5_moveit_config/ur5.srdf 文件中可以看到”manipulator” group 的 tip_link为 ee_link。<group name=“manipulator”><chain base_link=“base_link” tip_link=“ee_link”/></group> 在ur_description/urdf/ur5.urdf.xacro 中， <joint name=“${prefix}ee_fixed_joint” type=“fixed”><parent link=“${prefix}wrist_3_link”/><child link =“${prefix}ee_link”/><origin xyz=“0.0 0.0 0.0” rpy=“0.0 ${-pi/2.0} ${pi/2.0}”/></joint>

本文所用机器人在末端安装了hex-h传感器和rg6机械手，机器人 tcp变为rg6的指尖端点。因此moveit运动规划的 tip_link应修改。

由于机械手的urdf文件添加较为麻烦，此处采用简单粗暴的方法，直接修改ee_fixed_joint的坐标。

启动机器人运行程序，执行指令 rosrun tf tf_echo /wrist_3_link /tool0_controller,可以查看机器人TCP与 wrist_3_link 的坐标系转换关系，以此修改ee_fixed_joint即可。

<joint name=“${prefix}ee_fixed_joint” type=“fixed”><parent link=“${prefix}wrist_3_link”/><child link =“${prefix}ee_link”/><origin xyz=“0.0 0.0 0.315” rpy=“0.0 0.0 0.0”/></joint>