ros-noetic+ur5机械臂抓取仿真

1 moveit配置

接上次在机械臂的urdf文件（机械臂外型描述文件）中加入夹爪后，还需要通过Moveit来配置机械臂和夹爪的碰撞检测和控制器。后面加了一个深度相机。

第一步：加载编写好的urdf文件，加载成功过后，右边会出现一个机械臂模型。

第二步：生成自碰撞矩阵 Self-Collision Matrix

        默认的碰撞免检矩阵生成器搜索机器人所有关节，碰撞检测是检测两个连杆是否有接触，这个碰撞免检矩阵是可以安全地关闭检查，从而减少行动规划的处理时间。比如相邻的两个连杆因为是通过关节连接起来的，但实际上这不是碰撞。所以可以不用去费时间检查。

第三步：添加虚拟关节Virtual joint

添加虚拟关节主要是把机械臂关联到world。这里我们只需要定义一个虚拟关节把base_link关联到world，从而定义机械臂基座和world的坐标系关系。

第四步：添加规划组Planning Group

MoveIt通过定义规划组（planning group）来语义上定义机机械臂的各个部分（如手臂，末端执行器等）。这是MoveIt中一个很重要的概念。简单来说就是定义某些关节为一个组合并起一个名字。这里我添加了两个规划组，一个是arm，一个是gripper，后面使用moveit提供的python接口会用到，arm控制机械臂运动，gripper控制夹爪运动。

第五步：定义机械臂的一些姿态

第六步：定义末端执行器，这里我用的是gripper组连接到机械臂末端上

第七步：被动关节设置，因为机械臂没有被动关节，所以可以跳过

第八步：设置控制器，自动生成arm和gripper的控制器

第九步：加上作者信息和生成moveit配置文件

2 仿真中遇到的问题

1.夹爪控制器控制失败

自己定义的命名空间是“gripper”，而通过Moveit生成的控制器名字是“gripper_controller”，所以导致在Gazebo中夹爪控制失败。改成一致就行了。

除此之外，还要修改ros_controllers.yaml文件中的命名空间。

然后在控制器管理launch文件中加载修改过的控制器。

2.夹爪在抓方块过程中会出现夹不住的情况，方块滑落

需要在Gazebo仿真中写一个插件，这个插件是Gazebo官方提供的防脱落插件，在该插件中需要调试标签<release_tolerance>0.005</release_tolerance>的参数。使其达到合适的值，参数过小会出现滑落的现象，参数过大则方块会黏在夹爪上。

新建一个插件的xacro文件，插件代码如下

<?xml version="1.0" encoding="UTF-8"?>
<root xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
	xmlns:xacro="http://ros.org/wiki/xacro">


<!-- MACRO FOR THE ROBOT ARM ON THE TABLE-->
	<xacro:macro name="gzplugin_grasp_fix">
		<gazebo>
			<plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
				<!--
				NOTE: The finger tips are linked together with the links before, because they are connected with a
				fixed joint. Gazebo makes one whole link out of this automatically. When listing the 9_*_tip links
				here, they won't be found in the SDF.
				-->
            	<arm>
               		<arm_name>UR5Arm</arm_name>

					<!-- 0.01977<0.0198<0.01999 -->
					<palm_link> robotiq_85_left_inner_knuckle_link </palm_link>
					<gripper_link> robotiq_85_left_finger_tip_link </gripper_link>
					<palm_link> robotiq_85_right_inner_knuckle_link </palm_link>
					<gripper_link> robotiq_85_right_finger_tip_link </gripper_link>            
            	</arm>

				<forces_angle_tolerance>100</forces_angle_tolerance>
				<update_rate>20</update_rate>
				<grip_count_threshold>1</grip_count_threshold>
				<max_grip_count>3</max_grip_count>
				<release_tolerance>0.006</release_tolerance> <!-- 0.01977<0.0198<0.01999 -->
				<disable_collisions_on_attach>false</disable_collisions_on_attach>
				<contact_topic>__default_topic__</contact_topic>
			</plugin>
		</gazebo>
	</xacro:macro>

</root>

然后导入插件文件

3 利用moveit提供的python接口编写python来实现机械臂抓取

控制代码如下：

#! /usr/bin/env python
import sys
import rospy
import moveit_commander
# import geometry_msgs
# import tf


def graspRed():
    # 针对规划组进行控制
    arm_group = moveit_commander.move_group.MoveGroupCommander("arm")
    hand_group = moveit_commander.move_group.MoveGroupCommander("gripper")

    arm_group.set_named_target("home")
    plan = arm_group.go()
    print("Point1_home")

    # Open
    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("Point2_open_gripper")

    pose_target = arm_group.get_current_pose().pose

    # 移动到红色方块上方
    pose_target.position.x = 0.4
    pose_target.position.y = 0.0
    pose_target.position.z = pose_target.position.z+0.06

    arm_group.set_pose_target(pose_target)
    arm_group.go(wait=True)
    print("Point3_Red_overhead")

    # 落下
    pose_target.position.x = pose_target.position.x - 0.01
    pose_target.position.y = -0.01
    pose_target.position.z = pose_target.position.z-0.14

    arm_group.set_pose_target(pose_target)
    arm_group.go(wait=True)
    print("Point4_down")
    # 抓取
    hand_group.set_named_target("grasp")
    plan = hand_group.go()
    print("Point5_grasp")
    # 抬高
    pose_target.position.z = pose_target.position.z + 0.1
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point6_up")
    # 移动
    # pose_target.position.z = pose_target.position.z - 0.1
    pose_target.position.x = 0.55
    # pose_target.position.y = 0.2
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point7_xy")
    # 落下
    pose_target.position.z = pose_target.position.z - 0.08
    # pose_target.position.x = 0.8
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point7_down")
    # 放开夹抓
    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("Point8_open")
    # 抬高
    pose_target.position.y = 0.2
    pose_target.position.z = pose_target.position.z + 0.1
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point9_up")

    # 抓取绿色方快
    pose_target.position.x = 0.4 - 0.02
    pose_target.position.y = pose_target.position.y - 0.01
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point_green")
    # 落下
    pose_target.position.z = pose_target.position.z - 0.12
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Pointgreen_down")
    # 抓取
    hand_group.set_named_target("grasp")
    plan = hand_group.go()
    print("Pointgreen_grasp")
    # 抬高
    pose_target.position.z = pose_target.position.z + 0.11
    # pose_target.position.x = 0.8
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Pointgreen_up")

    pose_target.position.y = 0
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Pointy")
    
    pose_target.position.z = pose_target.position.z - 0.09
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Pointgreen_down")

    # 放开夹抓
    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("Pointgreen_open")

    pose_target.position.z = pose_target.position.z + 0.11
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Pointgreen_up")

    # 移动到蓝色方块上方
    pose_target.position.x = 0.4 - 0.01
    pose_target.position.y = -0.2 - 0.01
    pose_target.position.z = pose_target.position.z+0.06
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("PointBlue_overhead")
    # down
    pose_target.position.z = pose_target.position.z - 0.19
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("PointBlue_down")
    # 抓取
    hand_group.set_named_target("grasp")
    plan = hand_group.go()
    print("PointBlue_grasp")
    # up
    pose_target.position.z = pose_target.position.z + 0.1
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("PointBlue_up")
    # 移动到原绿色方块上方
    pose_target.position.x = 0.4
    pose_target.position.y = 0.2
    pose_target.position.z = pose_target.position.z+0.06
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("PointBlue_xyz")

    # down
    pose_target.position.z = pose_target.position.z - 0.13
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("PointBlue_down")

    # 放开夹抓
    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("PointBlue_open")

    # up
    pose_target.position.z = pose_target.position.z + 0.1
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("PointBlue_up")

    arm_group.set_named_target("home")
    plan = arm_group.go()
    print("home")
    rospy.sleep(5)
    moveit_commander.roscpp_initializer.roscpp_shutdown()

if __name__ == "__main__":
    #  初始化moveit_commander，创建一个node
    moveit_commander.roscpp_initializer.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_grasp', anonymous=True)

    # 创建一个RobotCommander对象。RobotCommander是针对整个机器人的控制
    robot = moveit_commander.robot.RobotCommander()
    graspRed()

4 效果演示

ur5+Gazebo抓取仿真_哔哩哔哩_bilibili

在贴上仿真环境吧，就是桌子和凳子。

最近想把这个代码上传到github，但是出了点问题上传不了，有需要的私信吧。

下期实现视觉定位抓取。

<?xml version="1.0" ?>

<sdf version="1.6">

  <world name="ur5_cubes">

    <gui>
      <camera name="user_camera">
        <pose>1.4 -2.3 1.4 0.0 0.25 1.9</pose>
      </camera>
    </gui>

    <gravity>0 0 -9.81</gravity>

    <physics name="default_physics" default="0" type="ode">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>

    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>

    <!-- Light Source -->
    <include>
      <uri>model://sun</uri>
    </include>

    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
      <pose>0 0 0 0 0 0</pose>
    </include>

    <!-- A gazebo links attacher -->
    <!-- <plugin name="ros_link_attacher_plugin" -->
    <!--         filename="libgazebo_ros_link_attacher.so"/> -->

    <!-- The robot base -->
    <model name="ur5_base">
      <static>true</static>

      <link name="top_plate">
        <pose>0 0 0.58 0 0 0</pose>
        <collision name="top_plate_collision">
          <geometry>
            <box>
              <size>0.5 0.5 0.02</size>
            </box>
          </geometry>
          <surface>
            <contact>
              <collide_bitmask>0x01</collide_bitmask>
            </contact>
          </surface>
        </collision>
        <visual name="top_plate_visual">
          <geometry>
            <box>
              <size>0.5 0.5 0.02</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="leg1">
        <pose>0.22 0.22 0.285 0 0 0</pose>
        <collision name="leg1_collision">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
        </collision>
        <visual name="leg1_visual">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="leg2">
        <pose>-0.22 0.22 0.285 0 0 0</pose>
        <collision name="leg2_collision">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
        </collision>
        <visual name="leg2_visual">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="leg3">
        <pose>0.22 -0.22 0.285 0 0 0</pose>
        <collision name="leg3_collision">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
        </collision>
        <visual name="leg3_visual">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="leg4">
        <pose>-0.22 -0.22 0.285 0 0 0</pose>
        <collision name="leg4_collision">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
        </collision>
        <visual name="leg4_visual">
          <geometry>
            <box>
              <size>0.04 0.04 0.57</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
    <!-- Table -->
    <!-- Table top is at (x,y,z) = (0.7, 0, 0.775) -->
    <!-- Table top size is 0.9x0.9 -->
    <model name="cafe_table">
      <static>true</static>
      <include>
        <uri>model://cafe_table</uri>
        <pose>0.7 0 0 0 0 0</pose>
      </include>
    </model>

    <!-- Cubes -->
    <model name="cube1">
      <pose>0.4 -0.2 0.775 0 0 0</pose>
      <link name="link">
        <pose>0 0 0.025 0 0 0</pose>
        <inertial>
          <mass>0.0565</mass>
          <inertia>
            <ixx>2.3541666666666672e-05</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>2.3541666666666672e-05</iyy>
            <iyz>0</iyz>
            <izz>2.3541666666666672e-05</izz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.05 0.05 0.05</size>
            </box>
          </geometry>
          <surface>
            <contact>
              <poissons_ratio>0.347</poissons_ratio>
              <elastic_modulus>8.8e+09</elastic_modulus>
              <ode>
                <kp>100000</kp>
                <kd>1</kd>
                <max_vel>1.0</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
            <friction>
              <torsional>
                <coefficient>1.0</coefficient>
                <use_patch_radius>0</use_patch_radius>
                <surface_radius>0.05</surface_radius>
              </torsional>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.05 0.05 0.05</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
    </model>

    <model name="cube2">
      <pose>0.4 0.2 0.775 0 0 0</pose>
      <link name="link">
        <pose>0 0 0.025 0 0 0</pose>
        <inertial>
          <mass>0.0565</mass>
          <inertia>
            <ixx>2.3541666666666672e-05</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>2.3541666666666672e-05</iyy>
            <iyz>0</iyz>
            <izz>2.3541666666666672e-05</izz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.05 0.05 0.05</size>
            </box>
          </geometry>
          <surface>
            <contact>
              <poissons_ratio>0.347</poissons_ratio>
              <elastic_modulus>8.8e+09</elastic_modulus>
              <ode>
                <kp>100000</kp>
                <kd>1</kd>
                <max_vel>1.0</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
            <friction>
              <torsional>
                <coefficient>1.0</coefficient>
                <use_patch_radius>0</use_patch_radius>
                <surface_radius>0.05</surface_radius>
              </torsional>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.05 0.05 0.05</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
    </model>

    <model name="cube3">
      <!-- <pose>0.4 0.2 0.775 0 0 0</pose> -->
      <pose>0.6 0.0 0.775 0 0 0</pose>
      <link name="link">
        <pose>0 0 0.025 0 0 0</pose>
        <inertial>
          <mass>0.0565</mass>
          <inertia>
            <ixx>2.3541666666666672e-05</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>2.3541666666666672e-05</iyy>
            <iyz>0</iyz>
            <izz>2.3541666666666672e-05</izz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.15 0.05 0.05</size>
            </box>
          </geometry>
          <surface>
            <contact>
              <poissons_ratio>0.347</poissons_ratio>
              <elastic_modulus>8.8e+09</elastic_modulus>
              <ode>
                <kp>100000</kp>
                <kd>1</kd>
                <max_vel>1.0</max_vel>
                <min_depth>0.001</min_depth>
              </ode>
            </contact>
            <friction>
              <torsional>
                <coefficient>1.0</coefficient>
                <use_patch_radius>0</use_patch_radius>
                <surface_radius>0.05</surface_radius>
              </torsional>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.05 0.05 0.05</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <!-- <name>Gazebo/WoodPallet</name> -->
              <name>Gazebo/WoodPallet</name>
            </script>
          </material>
        </visual>
      </link>
    </model>

  </world>

</sdf>