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

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

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

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

---

本文记录采用easy_handeye 和 aruco 在gazebo环境下对安装在UR10e上的D435相机进行手眼标定，对结果的准确性并未验证，只是熟悉流程。

一、功能包下载

在前两篇介绍的环境下，加装aruco_ros和easy_handeye两个功能包

pal-robotics/aruco_ros at melodic-devel (github.com)

IFL-CAMP/easy_handeye: Automated, hardware-independent Hand-Eye Calibration (github.com)

另外还需要下载一些标识码图片用于gazebo仿真环境搭建， 可以在下面的网站自己生成转成png格式。尽量用original版本的标识码（其他博主说的，反正都一样就听他的吧○|￣|_）

Online ArUco markers generator (chev.me)

在使用easy_handeye时遇到了需要安装transforms3d python包的问题，原因是默认安装的是Python3版本的transforms3d，而ros默认使用的是python2，最后通过安装python2版本的transforms3d解决问题。（Python的问题还是解决起来方便一点啊）

Collecting transforms3d
  Downloading https://files.pythonhosted.org/packages/05/bb/6d9bc2d2cc82c7984e0c6af586e561e8ca2e9e641483e617bbd256e5a8da/transforms3d-0.4.tar.gz (1.4MB)
    100% |████████████████████████████████| 1.4MB 716kB/s 
    Complete output from command python setup.py egg_info:
    Traceback (most recent call last):
      File "<string>", line 1, in <module>
      File "/tmp/pip-build-A8byOm/transforms3d/setup.py", line 8, in <module>
        import versioneer
      File "versioneer.py", line 370
        LONG_VERSION_PY: Dict[str, str] = {}
                       ^
    SyntaxError: invalid syntax

sudo -H pip2 install -U transforms3d==0.3.1  

二、gazebo环境搭建

这里主要参考dear小王子博主的gazebo9中在墙上添加二维码_dear小王子的博客-CSDN博客

前三部流程一样，主要是修改了他的model.sdf文件，他的墙太长了，我们只需要一个能放下一个aruco码的墙就行了。在~/.gazebo/models下保存的模型最终sdf文件为

<?xml version='1.0'?>
<sdf version='1.6'>
  <model name='aruco-wall'>
    <pose frame=''>0 0 0 0 -0 0</pose>
    <link name='Wall_0'>
      <collision name='Wall_0_Collision'>
        <geometry>
          <box>
            <size>1 0.1 1</size>
          </box>
        </geometry>
        <pose frame=''>0 0 1.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_0_Visual'>
        <pose frame=''>0 0 0.5 0 -0 0</pose>
        <geometry>
          <box>
            <size>1 0.1 1</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose frame=''>2e-06 0 0 0 -0 0</pose>
    </link>
  <joint type="fixed" name="aruco-1_joint">
      <pose>0 0 0 0 0 0</pose>
      <child>aruco-1</child>
      <parent>Wall_0</parent>
    </joint>
    <link name="aruco-1">
        <pose>0 0.05 0.5 0 0 0</pose>
            <visual name="aruco-1">
               <pose>0 0 0 0 0 0</pose>
              <geometry>
                <box>
                  <size>0.5 0.01 0.5</size>
                </box>
              </geometry>
              <material>
               <script>
                  <uri>file://media/materials/scripts/aruco.material</uri>
                  <name>Gazebo/aruco-1</name>
                </script>
		<ambient>1 1 1 1</ambient>
               </material>
           </visual>
     </link>
    <static>1</static>
  </model>
</sdf>

之后新建一个空白的gazebo环境将这个带有aruco码的墙插入到世界中生成.world文件即可。

三、launch文件编写

本节将编写两个launch文件用于仿真环境中手眼标定。

第一个是用来加载机械臂模型，标定环境，机械臂运动控制器和ArUco的launch文件。本质是在第一篇博客中的运动控制仿真使用的launch文件添加启动ArUco部分代码，和更换仿真环境。

<?xml version="1.0"?>
<launch>
  <arg name="paused" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="debug" default="false"/>
  <arg name="sim" default="true" />
  <arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
  <arg name="namespace_prefix" default="ur10e_rs_handeyecalibration" />
  <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.5"/>
  <arg name="marker_id" doc="The ID of the ArUco marker used" default="1"/>


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

  <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 -x 1.0
              -J shoulder_lift_joint -1.57
              -J elbow_joint -1.57
              -J wrist_2_joint 1.57
              -J wrist_3_joint 1.57 "
        output="screen" />
  
    <!-- Controller configuration -->
  <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"/>


  <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>

  <!-- start ArUco -->
  <node name="aruco_tracker" pkg="aruco_ros" type="single">
      <remap from="/camera_info" to="/d435/color/camera_info" />
      <remap from="/image" to="/d435/color/image_raw" />
      <param name="image_is_rectified" value="true"/>
      <param name="marker_size"        value="$(arg marker_size)"/>
      <param name="marker_id"          value="$(arg marker_id)"/>
      <param name="reference_frame"    value="d435_color_optical_frame"/>
      <param name="camera_frame"       value="d435_color_frame"/>
      <param name="marker_frame"       value="aruco_marker_frame" />
  </node>


  <include file="$(find easy_handeye)/launch/calibrate.launch" >
      <arg name="namespace_prefix" value="$(arg namespace_prefix)" />
      <arg name="start_rviz" value="false" />
      <arg name="eye_on_hand" value="true" />

      <arg name="tracking_base_frame" value="d435_color_optical_frame" />
      <arg name="tracking_marker_frame" value="aruco_marker_frame" />
      <arg name="robot_base_frame" value="base_link" />
      <arg name="robot_effector_frame" value="wrist_3_link" />

      <arg name="freehand_robot_movement" value="false" />
      <arg name="robot_velocity_scaling" value="0.5" />
      <arg name="robot_acceleration_scaling" value="0.2" />
  </include>
  <node name="gui_controller" pkg="rqt_joint_trajectory_controller" type="rqt_joint_trajectory_controller" />

</launch>

其中在最前面添加的marker_size 和 marker_id参数根据自己生成的码大小和id修改即可。主要是start ArUco之下的部分代码aruco_tracker节点中前两个是相机订阅话题，根据实际rostopic填写。

• reference_frame: marker 坐标系的参考坐标系，我们要获得的是marker 和相机的相对位姿，所以这里设置为相机坐标系，即和 camera_frame 一样。
• camera_frame: 相机坐标系
• marker_frame: marker 坐标系

easy_handeye节点中，在moveit配置中已经开启了rviz所以这里不再开启，我们是眼在手上所以将eye_on_hand设置成true。

tracking_base_frame: marker的坐标系的参考坐标系，这里写相机坐标系的参考坐标系，也就是相机坐标系。
tracking_marker_frame: marker 坐标系，即 aruco 设置的 marker_frame

第二个launch文件是修改aruco_ros->aruco_ros->launch中的single.launch，我是复制了一份新的进行修改。将其中对应的坐标系之类的修改成和第一个launch中一致。

<launch>

    <arg name="markerId"        default="1"/>
    <arg name="markerSize"      default="0.5"/>    <!-- in m -->
    <arg name="eye"             default="left"/>
    <arg name="marker_frame"    default="aruco_marker_frame"/>
    <arg name="ref_frame"       default="base_link"/>  <!-- leave empty and the pose will be published wrt param parent_name -->
    <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->


    <node pkg="aruco_ros" type="single" name="aruco_single">
        <remap from="/camera_info" to="/d435/color/camera_info" />
        <remap from="/image" to="/d435/color/image_raw" />
        <param name="image_is_rectified" value="True"/>
        <param name="marker_size"        value="$(arg markerSize)"/>
        <param name="marker_id"          value="$(arg markerId)"/>
        <param name="reference_frame"    value="$(arg ref_frame)"/>   <!-- frame in which the marker pose will be refered -->
        <param name="camera_frame"       value="d435_color_optical_frame"/>
        <param name="marker_frame"       value="$(arg marker_frame)" />
        <param name="corner_refinement"  value="$(arg corner_refinement)" />
    </node>

</launch>

四、仿真流程

首先启动上述的两个launch文件，可以再启动一个rqt_image_view，订阅aruco_single/result话题可以观察机械臂在该位置是否可以检测到aruco码。

启动launch后会打开gazebo，rviz，Hand-eye Calibrtation-rqt和automatic movement-rqt四个窗口以及我们自己打开的imageview窗口总计5个窗口（图里面是已经到达初始位置的）

 首先我是通过在rviz中的motionplanning运动到实现规划好的位置之后，点击automatic movement窗口下的check starting pose。可能会提示Can't calibrate from this position!，此时应该是初始位姿不太好，建议拖动rviz中机械臂末端小球寻找新的初始位姿，我的初始位姿下相机拍摄是画面是这样可以参考一下。

 之后的步骤就是

（1） 界面automatic movement-rqt依次点击 Next Pose，Plan，Execute，机械臂会移动至新的位置，若二维码在相机视野范围内，且能检测成功，则进行下一步
（2） 界面 Hand-eye Calibrtation-rqt 中点击 Take Sample，若 Samples 对话框中出现有效信息，说明第一个点标定成功
（3） 重复执行步骤 1 和步骤 2，直至 17 个点全部标定完毕
（4） 界面 Hand-eye Calibrtation-rqt 中点击 Compute，则 Result 对话框中会出现结果
（5） 界面 Hand-eye Calibrtation-rqt 中 Save，会将结果保存为一个 YAML 文件，路径为 ~/.ros/easy_handeye

系统自己规划的标定点可能采集不到aruco码的位置，这时候就在rviz中拖动一下末端小球用motionplanning规划执行一下即可，最后标定完所有位置后计算即可得到一个变换矩阵。

五、闲谈

最开始没有运行single.launch文件会在点击take sample时出现一个报错，内容大概是Error processing request: "camera_marker" passed to lookupTransform argument source_frame does not exist.后来在easy_handeyeGitHub的问题里找到了一个人的解决办法是再运行一个single.launch文件。猜测原因是该launch文件会发布对应的坐标轴信息吧，所以可以阻止报错。

之前眼在手上的手眼标定都是直接根据安装位置自己算的，感觉会更便捷吧○|￣|_。不知道这个的精度最后如何。