关于在ROS 中实现ur5与robotiq85的连接和控制的学习记录（笔记未成功版本）

• 目的：在rviz和gazebo中仿真ur5与robotiq85的连接和控制，最后通过外部发送数据到rviz中进行gazebo的仿真
• 环境：noetic。

第一部分 RVIZ部分：实现rviz中ur5与robotiq85的连接和控制

（一）：功能包下载

创建工作空间，下载好ur机械臂和robotiq夹爪相关工具包。

UR部分

cd ~/catkin_ws/
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot
git clone https://github.com/ros-industrial/ur_msgs.git src/ur_msgs
rosdep update
rosdep install --from-paths src --ignore-src -y
catkin_make

Robotiq部分
由于ros-industrial官方的包有些问题，所以使用如下链接

cd ~/catkin_ws
git clone https://github.com/jr-robotics/robotiq.git src/robotiq
catkin_make

（二）：修改ur.xacro模型

ur_description/urdf/ur.xacro中的代码修改为以下代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur">

   <!-- import main macro -->
   <xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro"/>
   <!--继承robotiq_arg2f_85宏-->
   <xacro:include filename="$(find robotiq_2f_85_gripper_visualization)/urdf/robotiq_arg2f_85_macro.xacro"/>

   <!-- parameters -->
   <xacro:arg name="joint_limit_params" default=""/>
   <xacro:arg name="kinematics_params" default=""/>
   <xacro:arg name="physical_params" default=""/>
   <xacro:arg name="visual_params" default=""/>
   <!-- legal values:
         - hardware_interface/PositionJointInterface
         - hardware_interface/VelocityJointInterface
         - hardware_interface/EffortJointInterface
   -->
   <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
   <xacro:arg name="safety_limits" default="false"/>
   <xacro:arg name="safety_pos_margin" default="0.15"/>
   <xacro:arg name="safety_k_position" default="20"/>

   <!-- arm -->
   <xacro:ur_robot
     prefix=""
     joint_limits_parameters_file="$(arg joint_limit_params)"
     kinematics_parameters_file="$(arg kinematics_params)"
     physical_parameters_file="$(arg physical_params)"
     visual_parameters_file="$(arg visual_params)"
     transmission_hw_interface="$(arg transmission_hw_interface)"
     safety_limits="$(arg safety_limits)"
     safety_pos_margin="$(arg safety_pos_margin)"
     safety_k_position="$(arg safety_k_position)"/>
     <!--定义fixed关节，将robotiq_arg2f_base_link连接到机械臂末端的tool0-->
     <joint name="ur_robotiq_joint" type="fixed">
       <parent link="tool0"/>
       <child link="robotiq_arg2f_base_link"/>
       <origin xyz="0 0 0" rpy="0 0 0"/>
     </joint>
 
     <!--调用robotiq_arg2f_85，加入夹爪的link和joint-->
     <xacro:robotiq_arg2f_85
       prefix=""
       transmission_hw_interface="$(arg transmission_hw_interface)"
     />
</robot>

打开robotiq/robotiq_2f_85_gripper_visualization/urdf/robotiq_arg2f_85_macro.xacro，将这段代码注释掉。如果单独仿真夹爪，要将其取消注释。

<!-- base link -->
<!-- <link name="${prefix}base_link"/> -->
<!-- <joint name="${prefix}base_link-${prefix}robotiq_arg2f_base_link" type="fixed"> -->
  <!-- <parent link="${prefix}base_link" /> -->
  <!-- <child link="${prefix}robotiq_arg2f_base_link" /> -->
<!-- </joint> -->

（三）：启动测试

cd ~/catkin_ws/
catkin_make
source devel/setup.bash
roslaunch ur_description view_ur5.launch

测试一下夹爪和模型是不是都可以控制了。

第二部分 gazebo部分：实现gazebo中ur5与robotiq85的连接和控制

（一）功能包下载

Robotiq2f85 gripper有被动关节（Mimic Joint），标准Gazebo不支持Mimic关节仿真， 所以需要安装一个插件。在一、实现rviz中ur5与robotiq85的连接和控制的基础上，下载plugins，然后catkin_make编译。

cd ~/catkin_ws/src
git clone https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins
cd ~/catkin_ws
catkin_make

（二）修改launch文件

gazebo的启动文件为fmauch_universal_robot/ur_gazebo/launch/ur5_bringup.launch。如一、中提到的，launch文件主要实现：加载参数、启动节点，下面一步步修改。这一步很多东西，要注意细节。

1.加载模型参数

我们需要修改的模型文件为：load_ur5.launch.xml—>load_ur.launch.xml—>ur_gazebo/urdf/ur.xacro。这句话我其实没看懂，我目前的操作是如下：

• load_ur5.launch.xml
  load_ur5.launch.xml是一个启动文件，用于加载UR5机器人的描述和相关参数。在你的情况下，它被用作 ur5_bringup.launch 中的一个包含文件，负责加载UR5机器人的描述和配置参数。

• load_ur.launch.xml
  load_ur.launch.xml 文件通常用于加载 UR5 机器人的描述和配置参数。它可能包含加载 URDF 文件、加载控制器参数文件等操作。在你的情况下，它被 load_ur5.launch.xml 文件引用，并且通过 pass_all_args="true" 选项传递所有参数。

• ur_gazebo/urdf/ur.xacro
  ur_gazebo/urdf/ur.xacro 文件是一个 UR5 机器人的 Xacro 描述文件，用于生成 UR5 机器人的 URDF 模型。在这个文件中，你可以定义机器人的链接、关节、传感器等部件，并且可以使用宏来简化描述。在文件最后的前面添加如下代码：

  <!-- 添加以下代码 -->
  <!-- 继承 robotiq_arg2f_85 宏 -->
  <xacro:include filename="$(find robotiq_2f_85_gripper_gazebo)/urdf/robotiq_arg2f_85_macro.xacro"/>
  <!-- 定义 fixed 关节，将 robotiq_arg2f_base_link 连接到机械臂末端的 tool0 -->
  <joint name="ur_robotiq_joint" type="fixed">
    <parent link="tool0"/>
    <child link="robotiq_arg2f_base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>
  <!-- 调用 robotiq_arg2f_85，加入夹爪的 link 和 joint -->
  <xacro:robotiq_arg2f_85_gazebo 
    prefix=""
    transmission_hw_interface="$(arg transmission_hw_interface)"
  />

</robot>

• robotiq_2f_85_gripper_gazebo/urdf/robotiq_arg2f_85_macro.xacro

打开robotiq_2f_85_gripper_gazebo/urdf/robotiq_arg2f_85_macro.xacro，修改以下两处。
将插件名字改为“libroboticsgroup_gazebo_mimic_joint_plugin.so”；

<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_1">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_2">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_3">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_4">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_5">

将100行左右这段代码注释掉。因为在fmauch_universal_robot/ur_gazebo/urdf/ur_macro.xacro中已经添加了这个插件，不再重复添加。

    <!--
      Inject Gazebo ROS Control plugin, which allows us to use ros_control
      controllers to control the virtual robot hw.
    -->
    <!--<gazebo>-->
      <!--<plugin name="ros_control" filename="libgazebo_ros_control.so">-->
      <!--</plugin>-->
    <!--</gazebo>-->

2.加载控制器参数

• robotiq_2f_85_gripper_gazebo/config/robotiq_2f_85_gripper_controllers.yaml

打开robotiq_2f_85_gripper_gazebo/config/robotiq_2f_85_gripper_controllers.yaml，修改gripper_controller的类型和关节。

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: &loop_hz 125

gripper_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - finger_joint

• ur_gazebo/config/ur5_controllers.yaml

修改ur_gazebo/config/ur5_controllers.yaml。

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: &loop_hz 125
 
pos_joint_traj_controller:
  type: position_controllers/JointTrajectoryController
  joints: &robot_joints
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
    shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
    elbow_joint: {trajectory: 0.1, goal: 0.1}
    wrist_1_joint: {trajectory: 0.1, goal: 0.1}
    wrist_2_joint: {trajectory: 0.1, goal: 0.1}
    wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate: *loop_hz
  action_monitor_rate: 10
 
gripper_controller:
  type: position_controllers/JointTrajectoryController
  gains:
    finger_joint: {p: 10,  d: 0.1, i: 1, i_clamp: 1}
  joints: 
    - finger_joint 
  action_monitor_rate: 20
  goal_tolerance: 0.002
  max_effort: 100
  stall_velocity_threshold: 0.001
  stall_timeout: 1.0
 
joint_group_pos_controller:
  type: position_controllers/JointGroupPositionController
  joints: *robot_joints

• ur5_bringup.launch中修改controllers参数值
  修改ur_gazebo/launch中ur5_bringup.launch启动三个控制器：关节状态发布、机械臂关节控制、夹爪关节控制。

<arg name="controllers" default="joint_state_controller pos_joint_traj_controller gripper_controller" doc="Controllers that are activated by default."/>

• 新建gazebo_controller.yaml

在robotiq/robotiq_2f_85_gripper_gazebo/config文件夹新建gazebo_controller.yaml。

# Note: You MUST load these PID parameters for all joints that are using the
# PositionJointInterface, otherwise the arm + gripper will act like a giant
# parachute, counteracting gravity, and causing some of the wheels to lose
# contact with the ground, so the robot won't be able to properly navigate. See
# https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612
ros_control:
  pid_gains:
    # these gains are used by the gazebo_ros_control plugin
    finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
    # the following gains are used by the gazebo_mimic_joint plugin
    left_inner_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
 
    left_inner_finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
 
    right_outer_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
 
    right_inner_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
 
    right_inner_finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

• 加载pid参数和添加控制节点

ur5_bringup.launch中最后添加两行代码加载夹爪的pid参数和添加控制节点

<rosparam file="$(find robotiq_2f_85_gripper_gazebo)/config/gazebo_controller.yaml" command="load" />
<node name="gui_controller" pkg="rqt_joint_trajectory_controller" type="rqt_joint_trajectory_controller" />

启动测试

到这一步应该可以在gazebo里面看到加了夹爪的模型了。

cd ~/catkin_ws/
catkin_make
source devel/setup.bash
roslaunch ur_gazebo ur5_bringup.launch

但是我这里显示pid参数错误，而且也没办法进行控制一样。可能还有哪里有问题

第三部分 Moveit配置实现rivz与gazebo的联合控制

（一）修改ur.xacro

在使用moveit时需要导入机械臂模型，我们使用的是fmauch_universal_robot/ur_gazebo/urdf/ur.xacro。

在一和二修改完成的基础上，将fmauch_universal_robot/ur_gazebo/urdf/ur.xacro中的

<xacro:arg name="joint_limit_params" default=""/>
<xacro:arg name="physical_params" default=""/>
<xacro:arg name="kinematics_params" default=""/>
<xacro:arg name="visual_params" default=""/>

改为

<xacro:arg name="joint_limit_params" default="$(find ur_description)/config/ur5/joint_limits.yaml"/>
<xacro:arg name="physical_params" default="$(find ur_description)/config/ur5/physical_parameters.yaml"/>
<xacro:arg name="kinematics_params" default="$(find ur_description)/config/ur5/default_kinematics.yaml"/>
<xacro:arg name="visual_params" default="$(find ur_description)/config/ur5/visual_parameters.yaml"/>

（二）安装和启动moveit

sudo安装对应版本的moveit，这里ROS版本是noetic

sudo apt-get install ros-noetic-moveit

打开新终端

roscore

打开新终端，启动moveit配置助手

cd ~/catkin_ws
catkin_make
source devel/setup.bash
rosrun moveit_setup_assistant moveit_setup_assistant

（三）配置moveit

• 1. Start加载模型

点击Create New Moveit Configuration Package，点击Browse，选择catkin_ws/src/fmauch_universal_robot/ur_gazebo/urdf/ur.xacro，打开。

点击Load Files，窗口右侧出现机械臂和夹爪模型

• 2.Self-Collisions碰撞检测矩阵

点击Generate Collision Matrix，生成碰撞检测矩阵。

• 3.Virtual Joints虚拟关节

Add Virtual Joint，设置虚拟关节将机械臂base_link连接到world。

• 4.Planning Groups设置规划组

Add Group，分别添加两个group：manipulator（机械臂）和gripper（夹爪）：

• manipulator（机械臂）
  manipulator（机械臂）：Kinematic Solver（动力学求解器）使用moveit自带的KDLKinematicsPlugin，其他使用默认配置；
  确认Add Kin. Chain，添加连杆chain；
  确认Add Joints，添加关节。

• gripper（夹爪）
  gripper（夹爪）：使用默认配置；Add Joints，添加关节。
  如下图

• 5.Robot Poses定义姿势

可以定义一些姿势。

• 6.End Effectors末端执行器

将gripper设置为末端执行器。（当只有机械臂时，tool0为末端执行器。）

• 7.Passive Joints被动关节
  这里我暂时不知道用来干啥的，先这样吧
  

• 8.Controllers控制器

首先，点击Auto Add FollowJointsTrajectoryControllers For Each Planning Group自动生成controllers。

为了实现moveit对gazebo中机械臂的控制，我们需要确保命名空间一致，即控制器名称要统一。参考fmauch_universal_robot/ur_gazebo/config/ur5_controllers.yaml，设置两个controllers的Name和Type。

最后如下

• 9-11暂时跳过
• 11.添加名字和邮件
• 12. Configuration Files生成配置文件
      选择路径，我新建了ur5_gripper_moveit_config包，最后生成

（四）启动launch

首先，打开fmauch_universal_robot/ur_gazebo/launch/ur5_bringup.launch，将gui_controller注释掉。

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

然后，启动gazebo仿真、启动moveit运动规划、启动rviz。

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roslaunch ur_gazebo ur5_bringup.launch

source devel/setup.bash
roslaunch ur5_gripper_moveit_config move_group.launch

source devel/setup.bash
roslaunch ur5_gripper_moveit_config moveit_rviz.launch config:=true

（五）运行结果

这里还有些问题，我还要再改改，rviz和gazebo都成功导入了，但是之间联合控制有问题，不能再gazebo中运动，同时rviz只有夹爪可以动，应该还是有问题，目前的报错是因为gain的pid控制器倍数不对，再研究一下各个设定吧。

————————————————
本文参考了这位博主的，然后我自己测试后，
第一部分也确实成功了。
第二部分不完全成功。
第三部分也还有问题。

原文链接：https://blog.csdn.net/zhangarmy/article/details/133376429

链接