Franka机械臂与RealsenseD435深度相机手眼标定

准备工作：

        1、Ubuntu20.04，ros_neotic，实时内核

        2、franka机械臂，realsense d435深度相机

        3、标定码

        控制franka机械臂时需要安装实时内核，否则在手眼标定过程中执行机械臂控制文件时容易            报错，点击实时内核安装，参考该链接，针对自己的版本进行安装。

        标定码头位于aruco_ros/aruco_ros/etc文件夹下，一定要1:1下载，还可以自己下载需要的标          定码，网址如下：

        标定码：https://chev.me/arucogen/

        内参标定码：Camera Calibration Pattern Generator – calib.io

一、配置环境

参考文章配置realsense d435相机环境，并进行验证，成功启动相机后可进行后续操作。

1、安装vision_visp

进入深度相机 src 目录下，也就是与realsense-ros同级别目录下，执行以下操作

cd ~/catkin_ws/src #进入文件夹
git clone -b neotic-devel https://github.com/lagadic/vision_visp.git

 编译时，耗时较长。可删除以下两功能包，用于追追踪，不影响标定。

 2、安装aruco_ros

cd ~/catkin_ws/src
git clone -b neotic-devel https://github.com/pal-robotics/aruco_ros

3、安装 easy_handeye

cd ~/catkin_ws/src
git clone https://github.com/IFL-CAMP/easy_handeye

4、安装panda_moveit_config

cd ~/catkin_ws/src
git clone https://github.com/ros-planning/panda_moveit_config.git -b neotic-devel

5、为方便执行，将launch文件拆分，如下所示，本人使用如下图所示标定板

标定板自动生成网址：Online ArUco markers generator，生成标定码时，注意红框内的参数选择，选择Original ArUco,Marker ID和Marker size的参数后续launch文件会用到。

注意： tracking_base_frame就是相机的frame, tracking_marker_frame是标志物的frame, 这两者之间的关系由aruco_ros发布, 所以要对上; robot_base_frame就是机械臂底座, robot_effector_frame是末端, 这两个跟franka发布的要对上.

创建easy_panda.launch文件

<?xml version="1.0" ?>
<launch>
    <arg name="namespace_prefix" default="panda_eob_calib" /> 
    <include file="$(find easy_handeye)/launch/calibrate.launch">
        <arg name="eye_on_hand" value="true"/>
        <arg name="namespace_prefix" value="$(arg namespace_prefix)"/>
        <arg name="move_group" value="panda_arm" />
        <!--<arg name="move_group" value="panda_manipulator"  doc="the name of move_group for the automatic robot motion with MoveIt!" /-->
        <arg name="freehand_robot_movement" value="false"/>

        <!-- fill in the following parameters according to your robot's published tf frames -->
        <arg name="robot_base_frame" value="panda_link0"/>
        <arg name="robot_effector_frame" value="panda_link8"/>

        <!-- fill in the following parameters according to your tracking system's published tf frames -->
        <arg name="tracking_base_frame" value="camera_link"/>
        <arg name="tracking_marker_frame" value="aruco_marker_frame"/>
    </include>
 
</launch>

创建panda_realsense_eyeonbase.launch，内容如下

<?xml version="1.0" ?>
<launch>
    <arg name="namespace_prefix" default="panda_eob_calib"/>

    <!-- (start your robot's MoveIt! stack, e.g. include its moveit_planning_execution.launch) -->
    <include file="$(find panda_moveit_config)/launch/franka_control.launch">
        <arg name="robot_ip" value="172.16.0.2"/> <!-- set your robot ip -->
        <arg name="load_gripper" value="false"/>
    </include>        
    
    <!-- (start your tracking system's ROS driver) -->
    <!-- <include file="$(find realsense2_camera)/launch/rs_camera.launch"> </include>      -->

        <!-- fill in the following parameters according to your tracking system -->
        <arg name="markerId"        default="582"/>      <!-- set your marker id -->
        <arg name="markerSize"      default="0.1"/>    <!-- in m -->
        <arg name="eye"             default="left"/>
        <arg name="marker_frame"    default="aruco_marker_frame"/> 
        <arg name="ref_frame"       default=""/>  <!-- leave empty and the pose will be published wrt param parent_name -->
        <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->    
        <arg name="camera_frame" default="camera_link" />
        <arg name="camera_image_topic" default="/camera/color/image_raw" />
        <arg name="camera_info_topic" default="/camera/color/camera_info" /> 

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


    <!-- (start hand-eye-calibration) -->
    <!-- <include file="$(find easy_handeye)/launch/calibrate.launch">
        <arg name="eye_on_hand" value="true"/>
        <arg name="namespace_prefix" value="$(arg namespace_prefix)"/>
        <arg name="move_group" value="panda_arm" />
        <arg name="move_group" value="panda_manipulator"  doc="the name of move_group for the automatic robot motion with MoveIt!" /-->
        <!-- <arg name="freehand_robot_movement" value="false"/>

        fill in the following parameters according to your robot's published tf frames -->
        <!-- <arg name="robot_base_frame" value="panda_link8"/>
        <arg name="robot_effector_frame" value="panda_hand_tcp"/> -->

        <!-- fill in the following parameters according to your tracking system's published tf frames -->
        <!-- <arg name="tracking_base_frame" value="camera_link"/>
        <arg name="tracking_marker_frame" value="aruco_marker_frame"/>
    </include>  -->

    <!-- (publish tf after the calibration) -->
    <!-- roslaunch easy_handeye publish.launch eye_on_hand:=false namespace_prefix:=$(arg namespace_prefix) -->

</launch>

 <arg name="robot_ip" value="172.16.0.2"/>，ip要与机械臂实际ip相同。

对panda_realsense_eyeonbase.launch文件进行修改如下，分别对应的ID号和标签尺寸，这是根据选择并打印出来的标签实际决定的：

图中有一处错误，相机的编号与尺寸，应该是标定码的尺寸与编号

创建两个.launch文件，并修改好参数后将其启移动至如下图所示文件夹内

 二、编译

执行以下命令进行编译

cd ~/catkin_ws
catkin_make

 如果是新安装系统，执行编译会报很多错误，大多是因为缺少某些包，根据自己系统版本，逐个下载即可。

下面列出某些错误

1、Resource not found: franka_control

Resource not found: franka_control
ROS path [0]=/opt/ros/melodic/share/ros
ROS path [1]=/home/znc/catkin_ws/src
ROS path [2]=/opt/ros/melodic/share
The traceback for the exception was written to the log file

#解决方案：

sudo apt-get install ros-neotic-franka-control 

 2、cannot launch node of type [moveit_ros_move_group/move_group]: moveit_ros_move_group

ERROR: cannot launch node of type [moveit_ros_move_group/move_group]: moveit_ros_move_group
ROS path [0]=/opt/ros/melodic/share/ros
ROS path [1]=/home/znc/catkin_ws/src
ROS path [2]=/opt/ros/melodic/share

# 解决方案
sudo apt-get install ros-neotic-moveit

3、Could not find a package configuration file provided by "Franka"

Could not find a package configuration file provided by "Franka" (requested
  version 0.8.0) with any of the following names:

    FrankaConfig.cmake
    franka-config.cmake
 
# 解决方案
sudo apt-get install ros-noetic-franka-gripper

4、Could not find a package configuration file provided by "VISP"

Could not find a package configuration file provided by "VISP" with any of
  the following names:

    VISPConfig.cmake
    visp-config.cmake

# 解决方案
sudo apt-get install ros-neotic-visp

针对编译过程中的报错，逐个解决，错误多是缺少相关依赖，逐个安装即可。

三、执行标定

        准备工作：

                1、将相机固定于机械臂末端，通过数据线与电脑相连

                2、用网线将机械臂与电脑相连，修改好电脑与机械臂ip，确保二者能够通信

1、执行相机节点

cd ~/catkin_ws$ source devel/setup.bash
roslaunch realsense2_camera rs_camera.launch 

结果如图，相机节点启动成功

2、启动机械臂

将机械臂通电之后，机械臂底座显示黄色灯光，如下图所示，进入网页，解锁机械臂，网址输入：172.16.0.2，点击下图红框内按钮，解锁机械臂。

 

解锁成功后，机械臂底座显示蓝色灯光，只有在蓝色状态下才能执行程序，切记切记

 执行机械臂控制的launch文件

cd catkin_ws
source devel/setup.bash
roslaunch easy_handeye panda_realsense_eyeonbase.launch 

 

 执行报错，如下图所示，需要在控制页面激活fci

 

 启动成功如下图所示：

启动easy_panda.launch文件，执行成功后会出现以下三个窗口：

按以下操作步骤进行：

点击add

依次点击红框内内容，可观察相机已识别到标定码

同时，操作另外两个窗口

点击Check starting pose 弹出Ready to start:click on next pose即说明可以采样，随后点击Take Sample进行采样，之后点击Next Pose 再点击Plan,依次执行，采样十七次，点击Compute进行计算输出转换矩阵。

标定完成！