UR10e+D435+ag95夹爪 gazebo 仿真记录（一）

本文主要是介绍UR10e+D435+ag95夹爪 gazebo 仿真记录（一），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

学习仿真环境gazebo下利用UR10e机械臂搭建模拟平台，此博客用于记录搭建过程

搭建过程参考了很多博主的帖子和GitHub开源代码，对在此领域一起努力并且乐于分享的人表示感谢！！！

主要参考光头明明、总有一天全城的人都要高看我们和bug404_三位博主。最终自己功能包的主要参考了bug404_的ur_platform，感觉这个结构比较合理。

遵循尽量不改动GitHub下载的功能包原则方便代码的管理和修改。

(65条消息) UR5+robotiq_85_gripper GAZEBO模拟视觉抓取平台仿真-1_光头明明的博客-CSDN博客

(65条消息) gazebo+rviz配置UR5+dh_gripper（大寰）夹爪最详细教程_总有一天全城的人都要高看我们的博客-CSDN博客 (65条消息) ur机械臂 + robotiq gripper + robotiq ft sensor + gazebo + 连接真实机械臂 + 网页控制_ur robotiq gazebo_bug404_的博客-CSDN博客

运行环境：Ubuntu18.04+Ros-melodic+Gazebo9

一、预期实现的功能

二、ROS功能包

三、机械臂+夹爪+相机URDF文件编写

四、gazebo环境搭建

五、Moveit配置

六、gazebo+moveit launch文件编写

1.仿真世界加载

2.机械臂模型加载

3.控制器配置

七、使用python实现抓取

八、闲谈

未完待续。。。。。

一、预期实现的功能

目前通过仿真想实现的功能有：基于视觉的目标定位与抓取、相机手眼标定、IBVS部署等（后面再有啥想实现的再加吧，先就这么多）

本文先记录一下目标定位与抓取放置

二、ROS功能包

主要用的功能包有：

ag95夹爪：GitHub - DH-Robotics/dh_gripper_ros: ROS DH-Robotics meta-package

UR机械臂：GitHub - ros-industrial/universal_robot: ROS-Industrial Universal Robots support (https://wiki.ros.org/universal_robot)

D435相机：GitHub - rickstaa/realsense-ros-gazebo: Gazebo implementation of the realsense-ros repository

还有一些gazebo插件用于控制和防止夹爪散架，GitHub - JenniferBuehler/gazebo-pkgs: A collection of tools and plugins for Gazebo

三、机械臂+夹爪+相机URDF文件编写

本节主要是编写ur_platform.urdf.xacro文件。

该部分bug404_博主文章中有哔哩哔哩视频讲解建议看一看，up讲的还是非常好的。

直接贴出URDF文件。两个gazebo插件一个是为了抓放物体时方便夹住和脱离的（但是目前我的夹爪在gazebo中夹方块还是会滑出），另一个是用于ROS控制的。

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur_platform"><xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/><xacro:include filename="$(find ur_e_description)/urdf/common.gazebo.xacro" /><xacro:include filename="$(find ur_e_description)/urdf/ur10e.urdf.xacro" /><xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro" /><!--<xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro" />--><xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/><xacro:ur10e_robot prefix="" joint_limited="false"/><xacro:include filename="$(find dh_robotics_ag95_description)/urdf/dh_robotics_ag95_gripper.xacro" /><link name="world" /><joint name="world_joint" type="fixed"><parent link="world" /><child link = "base_link" /><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></joint><xacro:robotiq_ft300 prefix="" parent="tool0" ><origin xyz="0 0 0" rpy="0 0 0"/> </xacro:robotiq_ft300><xacro:realsense_d435  sensor_name="d435" parent_link="tool0" rate="10"><origin xyz="-0.1 0.0 0" rpy="0.0 ${-pi/2} 0.0" /></xacro:realsense_d435 ><xacro:dh_robotics_ag95_gripper   prefix="" parent="robotiq_ft_frame_id"><origin xyz="0 0 0" rpy="0 0 0 "/></xacro:dh_robotics_ag95_gripper><gazebo><plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so"><arm><arm_name>ur10_gripper</arm_name><palm_link> gripper_finger1_inner_knuckle_link </palm_link><gripper_link> gripper_finger1_finger_tip_link </gripper_link><palm_link> gripper_finger2_inner_knuckle_link </palm_link><gripper_link> gripper_finger2_finger_tip_link </gripper_link></arm><forces_angle_tolerance>120</forces_angle_tolerance><update_rate>10</update_rate><grip_count_threshold>1</grip_count_threshold><max_grip_count>3</max_grip_count><release_tolerance>0.005</release_tolerance><disable_collisions_on_attach>false</disable_collisions_on_attach><contact_topic>__default_topic__</contact_topic></plugin></gazebo><gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><robotNamespace>/ur_platform</robotNamespace></plugin></gazebo></robot>

虽说要减少在原文件的中的修改，但是必要的修改还是必须的(lll￢ω￢)。dh夹爪文件中需要修改dh_robotics_ag95_gripper.xacro文件，注释掉该部分代码。

    <gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> <robotNamespace>/</robotNamespace><legacyModeNS>true</legacyModeNS></plugin> </gazebo><link name="world"/><joint name="world_fixed" type="fixed"><origin xyz="0 0 0" rpy="0 0 0"/><parent link="world"/><child link="gripper_root_link"/></joint><link name="gripper_root_link"><visual><origin xyz="0 0 0.5" rpy="0 0 0"/><geometry><box size="0.1 0.1 1.0"/></geometry></visual><collision><origin xyz="0 0 0.5" rpy="0 0 0"/><geometry><box size="0.1 0.1 1.0"/></geometry></collision><inertial><mass value="1e2" /><origin xyz="0.0 0.0 0.0" /><inertia ixx = "0.000001" ixy = "0.000000" ixz = "0.000000"iyx = "0.000000" iyy = "0.000001" iyz = "0.000000"izx = "0.000000" izy = "0.000000" izz = "0.000001" /></inertial></link><xacro:dh_robotics_ag95_gripper prefix="" parent="gripper_root_link" ><origin xyz="0.05 0 0.85" rpy="${pi/2}  0 ${pi/2}"/></xacro:dh_robotics_ag95_gripper>

dh_robotics_ag95_gripper.transmission.xacro文件中修改第9行代码为

<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

我遇到了mimic_joint_plugin.cpp文件的编译问题，需要替换文件为该链接下的roboticsgroup/roboticsgroup_upatras_gazebo_plugins (github.com)

通过launch文件在rviz查看模型，可以看到都加入进来了。

四、gazebo环境搭建

首先搭建一个自己的gazebo仿真环境，可以放桌子啊凳子啊自己想要抓取的东西之类的，这部分参考了光头明明的环境，就是桌子上放了3个小盒子。

打开gazebo后就可以insert一些物体，如果直接insert的盒子太大了可以Edit->Model Editor，自己新建一些模型保存到指定路径之后就可以insert了。环境搭建完成后，可以在指定位置保存为.world文件。（如果点击另存为后界面灰色了但是没有弹出界面，隐藏gazebo在最大化就行了，反复这一步到保存好）

写一个launch文件看看自己的世界是怎么样的，我就搭建成这样了，距离啥的不满意可以直接在.world文件里直接修改。

<?xml version="1.0"?>
<launch><arg name="paused" default="false"/><arg name="gui" default="true"/><arg name="debug" default="false"/><!-- startup simulated world --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" default="$(find ur_platform_gazebo)/worlds/ur5_cubes.world"/><arg name="paused" value="$(arg paused)"/><arg name="gui" value="$(arg gui)"/></include>
</launch>

五、Moveit配置

通过roslaunch启动moveit配置助手，具体每一步的意思可以参考该博客，这一部分比较固定没有太多需要修改的部分，后续有问题可以在xxx_moveit_comfig->config->xxx.srdf文件中直接修改，或者重新在配置助手里加载该文件修改（我比较喜欢直接修改srdf文件，方便一点）(65条消息) MoveIt入门之——使用MoveIt配置助手生成MoveIt配置文件_沐棋的博客-CSDN博客

roslaunch moveit_setup_assistant setup_assistant.launch

六、gazebo+moveit launch文件编写

主要包括自己搭建的gazebo世界加载，机械臂urdf文件加载，控制器配置三大部分

1.仿真世界加载

我自己的世界名字是ur5_cubes.world，根据自己搭建的世界名字修改此处文件名即可，verbose可以看到gazebo加载时更多的控制台信息，paused模式开启时，可以更好的看清机械臂的初始位置，在该模式下进行运动规划要记得点下面的三角符号，gui设置为true。

  <include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="verbose" value="true" /><arg name="world_name" default="$(find ur_platform_gazebo)/worlds/ur5_cubes.world"/><arg name="paused" value="$(arg paused)"/><arg name="gui" value="$(arg gui)"/></include>

2.机械臂模型加载

第一段是加载自己写的urdf文件，第二部分是设置机械臂在gazebo世界中base_link的初始位置-x 位置(m)和机械臂初始位姿-J xxx_joint 角度(rad)。如果不采用paused模式，我都机械臂总是运动不到设定的初始位姿，可能是硬件不行吧，算不动，麻了。

  <include file="$(find ur_platform_description)/launch/ur_platform_upload.launch"><arg name="limited" value="$(arg limited)"/></include><!-- push robot_description to factory and spawn robot in gazebo --><node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.594-J shoulder_lift_joint -1.57 "output="screen" />

3.控制器配置

gazebo的物理引擎还是十分真实的，想要驱动机器人运行需要配置运动控制器。由于我们使用的是现成的工业机械臂，夹爪，相关的运动控制器官方都有，所以这里我们直接调用官方的controller.yaml，并做一些修改。

 <include file="$(find ur_gazebo)/launch/controller_utils.launch"/><rosparam file="$(find dh_robotics_ag95_gazebo)/controller/joint_state_controller.yaml" command="load"/><arg name="controller_config_file" default="$(find ur_e_gazebo)/controller/arm_controller_ur10e.yaml" doc="Config file used for defining the ROS-Control controllers."/><rosparam file="$(arg controller_config_file)" command="load"/><rosparam file="$(find dh_robotics_ag95_gazebo)/controller/gripper_controller_dh_robotics.yaml" command="load"/><node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>

具体修改主要是为控制器添加PID参数，在arm_controller_ur10e.yaml下添加

ur_platform/gazebo_ros_control/pid_gains: # Required because we're controlling an effort interfaceshoulder_pan_joint: {p: 100, d: 10, i: 0.01, i_clamp: 1}shoulder_lift_joint: {p: 100, d: 10, i: 0.01, i_clamp: 1}elbow_joint: {p: 100,  d: 10, i: 0.01, i_clamp: 1}wrist_1_joint: {p: 100,  d: 10, i: 0.01, i_clamp: 1}wrist_2_joint: {p: 100,  d: 10, i: 0.01, i_clamp: 1}wrist_3_joint: {p: 100,  d: 10.1, i: 0.01, i_clamp: 1}

gripper_controller_dh_robotics.yaml下添加

ur_platform/gazebo_ros_control/pid_gains: # Required because we're controlling an effort interfacegripper_finger1_joint: {p: 100, d: 1, i: 0.01, i_clamp: 1}

对于前缀ur_platform/gazebo_ros_control/pid_gains，最开始我写成gazebo_ros_control/pid_gains终端报错缺少ur_platform/gazebo_ros_control/pid_gains，并且无法通过rivz中的motion planning驱动，修改成现在这样，终端会报错缺少gazebo_ros_control/pid_gains，但是不影响运动控制，就无视这个error吧。

最后将moveit引入，其中moveit_planning_execution.launch是自己编写的，主要就是调用move_group.launch。由于我们这里用了moveit_rviz.launch其中arg name使用的是config，于原文件的rviz_conf(好像是这个参数名)不一样会报错，选择一个修改成另一个的样子传参就不会出错了。

  <include file="$(find ur_platform_moveit_config)/launch/moveit_planning_execution.launch"><arg name="debug" default="$(arg debug)" /><arg name="sim" default="$(arg sim)" /></include><include file="$(find ur_platform_moveit_config)/launch/moveit_rviz.launch"><arg name="debug" default="$(arg debug)" /><arg name="config" default="true" /></include>

还有就是如果运行时出现[execution_type]参数相关的错误，将xxx_moveit_config->trajectory_execution.launch.xml中有关execution_type的参数注释掉就行。这个问题一开始碰到了，但是后面重新配置moveit之后，生成的文件又没有这个问题了，不知道为啥。

启动launch文件就可以实现rviz和gazebo运动的互通了，可以在launch中加一句，可以用gui界面控制各个关节运动。也可以在rviz中使用motion planning驱动机械臂到达moveit配置中提前设定的位置。

<node name="gui_controller" pkg="rqt_joint_trajectory_controller" type="rqt_joint_trajectory_controller" />

七、使用python实现抓取

我就直接贴出代码吧，也是从光头明明博主的GitHub上直接拿来的，代码比较简单，也是示教法确定的位置。set_name_target()中的参数是moveit配置时自己命名的路点名字，想增加修改可以直接在srdf文件中删改，示教可以用上一节的gui界面一点一点调整，笨有笨的方法嘛。

#! /usr/bin/env python
import sys
import rospy
import moveit_commander
import geometry_msgs
import tfmoveit_commander.roscpp_initializer.roscpp_initialize(sys.argv)
rospy.init_node('move_group_grasp', anonymous=True)
robot = moveit_commander.robot.RobotCommander()arm_group = moveit_commander.move_group.MoveGroupCommander("manipulator")
hand_group = moveit_commander.move_group.MoveGroupCommander("gripper")#hand_group.set_named_target("close")
#plan = hand_group.go()arm_group.set_named_target("base")
plan = arm_group.go()print("Point 1")# Open
#hand_group.set_joint_value_target([9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05, 9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05])
#hand_group.go(wait=True)
#print("Point 2")
hand_group.set_named_target("open")
plan = hand_group.go()
print("Point 2")pose_target = arm_group.get_current_pose().pose# Block point
pose_target.position.x = 0.69
pose_target.position.y = 0.0
pose_target.position.z = pose_target.position.zarm_group.set_pose_target(pose_target)
arm_group.go(wait=True)
print("Point 3")# Block point
pose_target.position.x = 0.69
pose_target.position.y = 0.0
pose_target.position.z = pose_target.position.z-0.23arm_group.set_pose_target(pose_target)
arm_group.go(wait=True)
print("Point 4")hand_group.set_named_target("close")
plan = hand_group.go()
print("Point 5")pose_target.position.z = pose_target.position.z+0.1
arm_group.set_pose_target(pose_target)
plan = arm_group.go()
print("Point 6")pose_target.position.z = pose_target.position.z
pose_target.position.x = 1.0
arm_group.set_pose_target(pose_target)
plan = arm_group.go()
print("Point 7")hand_group.set_named_target("open")
plan = hand_group.go()
print("Point 8")rospy.sleep(5)
moveit_commander.roscpp_initializer.roscpp_shutdown()

八、闲谈

之前采用robotiq85夹爪，散架问题十分严重，查了很多博客说是要重新配置mimc插件之类的，最后也没有搞好。后来看到一个博客说是，robotiq85夹爪官方没有给gezebo仿真相关的配置，ag95是配置了的，所以换成了该夹爪用于仿真。（后续也用不到夹爪，只是仿真学习无所谓的(╯‵□′)╯︵┻━┻）

换成ag95后，在gazebo仿真中还是会出现散架的问题，后来配置了PID参数后就好了，现在看来当时的robotiq85夹爪可能就是缺了最后PID配置吧🤔。

可以看到我的urdf中是加入了gazebo_grasp_fix插件，但是还是无法解决抓取不了物体的问题，现在的抓取是这样，有知道原因的大佬可以教教我吗，孩子麻了，这个bug修了三四天都不行，我也在GitHub上提问了，详细原因可以移步GitHub。Can't attach object using dh_robotics_ag95_gripper.Object slips away when grasping. · Issue #58 · JenniferBuehler/gazebo-pkgs (github.com)