学习仿真环境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
目录
一、预期实现的功能
目前通过仿真想实现的功能有：基于视觉的目标定位与抓取、相机手眼标定、IBVS部署等（后面再有啥想实现的再加吧，先就这么多）
本文先记录一下目标定位与抓取放置
二、ROS功能包
主要用的功能包有：
ag95夹爪：GitHub - DH-Robotics/dh_gripper_ros: ROS DH-Robotics meta-package
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 interface
shoulder_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 interface
gripper_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 tf
moveit_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.z
arm_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.23
arm_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)
8157
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
