准备工作
系统：双系统 Ubuntu 1804
ROS： Melodic（默认已经安装好）
OpenCV：
- Opencv-python v4.1.0.25 （pip安装）
- Opencv v3.4.15（系统编译安装）
realsense D435i驱动：（默认已经安装好） - Intel RealSense Viewer v2.54.2
- ROS realsense-ros-2.3.2.tar.gz
Gluon：（默认已经安装好，参考上一篇博客）
linux驱动：GitHub - mintasca/innfos-cpp-sdk
linux驱动软件（如果有规划的位姿超出范围并且执行后，电机会闪红灯报错，报错后需要在该软件中手动恢复）GitHub - mintasca/Minta-Actuator-Studio-linux
Gluon的ROS驱动 GitHub - mintasca/ros_gluon
aruco：GitHub - pal-robotics/aruco_ros at melodic-devel
easy_hand_eye：GitHub - IFL-CAMP/easy_handeye: Automated, hardware-independent Hand-Eye Calibration
step1.0、使用moveit_setup_assistant 配置机械臂
- step1.0.1 修改urdf文件
因为我拿到手的机械臂与URDF中的零点位置不同，所以更改一下配置文件~/innfos_ros/src/gluon/urdf/gluon.urdf
大约在113行左右，axis_joint_1的关节角度限制改一下
改成
<limit
lower="-4.34"
upper="1.2"
effort="30"
velocity="3.14" />
然后启动机械臂配置助手
roslaunch moveit_setup_assistant setup_assistant.launch
- step1.1 新建一个Moveit config包
- step1.2 选择刚刚修改好的urdf文件
- step1.3 点击Load Files
加载完成如图所示
-
step1.4 点击Generate Collision Matrix 生成自碰撞矩阵
-
step1.5 添加虚拟关节，world
-
step1.6 添加规划组，子关节
-
step1.7 添加预设位姿，我就添加了一个，可以多添加几个
-
step1.8 End Effectors 添加末端执行器，因为我这里只有机械臂，还没安装夹爪，所以这里先不设置
-
step1.9 Passive Joints 被动关节，这里没有，如果安装了夹爪那么夹爪的爪子就是被动关节
-
step1.10 ROS Control 选择自动生成
-
step1.11 Simulation与3D Perception 跳过，标定完后再设置
-
step1.12 填入作者信息
-
step1.13 生成包，注意选择路径
-
setp1.14 退出配置助手，刷新环境变量
source ~/.bashrc
- step1.15 修改生成的demo.launch
<launch>
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find GL2_moveit_config)/default_warehouse_mongo_db" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<arg name="use_gui" default="false" />
<!-- <arg name="use_rviz" default="true" /> -->
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find GL2_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- If needed, broadcast static tf for robot root -->
<!-- Real robot connected -->
<include file="$(find gluon_control)/launch/gluon_control.launch">
</include>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find GL2_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="false"/>
<!-- <arg name="fake_execution_type" value="$(arg fake_execution_type)" /> -->
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)"/>
<!-- <arg name="load_robot_description" value="$(arg load_robot_description)"/> -->
</include>
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<include file="$(find GL2_moveit_config)/launch/moveit_rviz.launch" >
<arg name="rviz_config" value="$(find GL2_moveit_config)/launch/moveit.rviz"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(find GL2_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
</include>
</launch>
step1.16 验证是否可用 GL2_moveit_config是我使用配置后的包名字，不建议使用大写字母开头，会报错
机械臂上电并使用该命令，同时用手扶机械臂，防止机械臂初始化过程中未初始化的电机关节由于重力作用下落损坏设备。
roslaunch GL2_moveit_config demo.launch
此时机械臂会进入零点位置
在rviz中随意拖动设备，plan并execute
检查是否会遇到执行失败的问题，如图所示。
如果有许多角度都失败了那么重启一下demo.launch
不然下面标定过程中会卡在 Cannot calibrate from current position 上
机械臂使用完成后进入innfos_arm/innfos-gluon-controller/关闭电源使能
cd ~/innfos_arm/innfos-gluon-controller/
. environment
./setrobot t GL_2L6_4L3 # GL_2L6_4L3需要执行这个,不执行问题也不大还没有遇到
./robotserver mode0 # 关闭电源使能
step2.0 编译标定程序
-
step2.1 下载并打印标定板
Online ArUco markers generator (chev.me)下载并打印标定板，注意参数选择
-
step2.2 新建一个工作空间编译标定程序
mkdir -p ~/test_aruco_easy_hand/src && cd ~/test_aruco_easy_hand/src
git clone https://github.com/pal-robotics/aruco_ros.git
git clone https://github.com/IFL-CAMP/easy_handeye.git
修改参数
cd ~/test_aruco_easy_hand/src/aruco_ros/launch
vim single_realsense.launch
粘贴入下面内容
<launch>
<arg name="markerId" default="582"/>
<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="camera_color_frame"/> <!-- 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="/camera/color/camera_info" />
<remap from="/image" to="/camera/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="camera_color_frame"/>
<param name="marker_frame" value="$(arg marker_frame)" />
<param name="corner_refinement" value="$(arg corner_refinement)" />
</node>
</launch>
参数说明
-
markerId：使用的Marker标签的ID，这里在准备时打印的Marker标签ID为582
-
markerSize：Marker标签的实际大小，单位为m，这里在准备时打印的Marker标签实际大小为10厘米即0.1m
-
ref_frame：参考坐标系名称，这里选择Realsense的camera_color_frame作为参考坐标系
-
<remap from=“/camera_info” to=“/camera/color/camera_info” />：将/camera_info重映射为对应Realsense实际发布的相应的Topic即/camera/color/camera_info
-
<remap from=“/image” to=“/camera/color/image_raw” />：将/image重映射为对应Realsense实际发布的相应的Topic即/camera/color/image_raw
-
camera_frame：相机坐标系，修改为实际的相机坐标系camera_color_frame
-
step2.3 配置easy_handeye
cd ~/test_aruco_easy_hand/src/easy_handeye/easy_handeye/launch/
vim eye_in_hand_calibrate.launch
粘贴如以下内容
<?xml version="1.0"?>
<launch>
<!-- 生成标定文件的名称 -->
<arg name="namespace_prefix" default="gluon_rs_d435i" />
<!-- gluon机械臂MoveIt!配置的move_group为gluon，所以修改为gluon -->
<arg name="move_group" default="gluon" />
<!-- start easy_handeye -->
<include file="$(find easy_handeye)/launch/calibrate.launch" >
<arg name="namespace_prefix" value="$(arg namespace_prefix)" />
<arg name="move_group" value="$(arg move_group)" />
<!-- 这里使用眼在手上的方式进行标定，所以此处改成true -->
<arg name="eye_on_hand" value="true" />
<!--tracking_base_frame为realsense的相机坐标系-->
<arg name="tracking_base_frame" value="camera_color_frame" />
<!--tracking_marker_frame对应aruco_ros包中single_realsense.launch中的marker_frame的值-->
<arg name="tracking_marker_frame" value="aruco_marker_frame" />
<!--robot_base_frame为机器人基座坐标系-->
<arg name="robot_base_frame" value="dummy" />
<!--robot_effector_frame为工具坐标系，如夹爪，吸盘等，但实际的机器人模型未添加夹爪，所以这里设置为末端关节5_Link-->
<arg name="robot_effector_frame" value="5_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>
</launch>
PS：这里的机械臂末端是实际链接深度相机的关节，我的
可以看到我的深度相机实际上是连接在第五个电机上的，即其坐标是相对于5_Link固定的，所以我这里写的是5_Link
链接机械臂与深度相机的模型来自于从零开始的机械臂yolov5抓取gazebo仿真（一）Lord_ZYX的博客-CSDN博客
修改完成后编译
cd ~/test_aruco_easy_hand/
catkin_make
echo "source ~/test_aruco_easy_hand/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
step3.0 自动标定
- step3.1 启动Realsense、aruco、机械臂、easy_handeye 节点
# 启动深度相机
roslaunch realsense2_camera rs_camera.launch
# 启动aruco
roslaunch aruco_ros single_realsense.launch
# 打开image_view 查看aruco发布的图像话题
rosrun image_view image_view image:=/aruco_single/result
# 启动机械臂moveit控制程序
roslaunch GL2_moveit_config demo.launch
# 然后调整机械臂位姿，直到aruco发布的图像话题中可以看到标定板在视野中央
# 启动easy_handeye
roslaunch easy_handeye eye_in_hand_calibrate.launch
如图
-
step3.2 重复1、2、3、4。只要画面中能看到标定板的坐标信息就点击4采样
-
step3.3 直到十七个点标定完，点击计算
-
step3.4 可以点击保存，标定信息保存在
~/.ros/easy_handeye/下面
PS ：遇到
Cannot calibrate from current position
的问题
- 要么就调整一下机械臂位姿，重启一下
GL2_moveit_config demo.launch - 或者干脆使用手动标定的办法
附上两次标定的结果
# 第一次
translation:
x: 0.00186935102131
y: 0.0193147763718
z: 0.0783182224774
rotation:
x: -0.481285930881
y: 0.303509486976
z: -0.454033133706
w: 0.685638211849
# 第二次
translation:
x: 0.0157418883308
y: 0.0423640147247
z: 0.152850956679
rotation:
x: -0.530311271262
y: 0.463840825509
z: -0.473198006338
w: 0.528871715033
step4.0 手动标定
由于在 Cannot calibrate from current position 这个问题上卡了两天。
于是打算使用手动标定的办法
具体步骤
- step4.1 手动控制机械臂，使aruco发布的图像话题中可以看到标定板在视野中，然后每调整一个动作订阅一下
/aruco_single/poserosto
rostopic echo /aruco_single/poserosto
- step4.2 记录一下相机与标定板的position与orientation
header:
seq: 12526
stamp:
secs: 1697013561
nsecs: 250904322
frame_id: "camera_color_frame"
pose:
position:
x: -0.0191426966339
y: 0.00880137924105
z: 0.211938127875
orientation:
x: -0.016056670143
y: 0.998521559028
z: 0.0419401089597
w: -0.0306252634272
- step4.3 然后使用
moveit_commander_cmdline.py记录一下机械臂末端位姿
rosrun moveit_commander moveit_commander_cmdline.py
use gluon
current # 查看当前机械臂末端相对于基坐标的position与orientation
（这里记得需要在moveit中gluon规划组的末端改为5_Link）（忽略我下面的Link6 这里只讲方法）
joints = [0.0231194779469 0.225352582548 -1.86336170163 -2.08869864822 -1.5825253568 6.61477915356e-05]
6_Link pose = [
header:
seq: 0
stamp:
secs: 1697013505
nsecs: 791723966
frame_id: "dummy"
pose:
position:
x: 0.080115429479
y: 0.188752756384
z: 0.268797094909
orientation:
x: -2.89505769747e-05
y: -1.76295785108e-05
z: 0.703068203569
w: 0.71112242264 ]
6_Link RPY = [-6.596440111243088e-05, 1.5634883131054623e-05, 1.5594058788530805]
- step4.4 累计记录十六对位姿后
新建文件夹
mkdir -p ~/eyehand_cul/src && cd eyehand_cul/src
vim eyehand_cul.cpp
将位姿按照x、y、z、w、x、y、z填入下面代码，然后复制进eyehand_cul.cpp里面
#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <iterator>
#include <algorithm>
using namespace cv;
using namespace std;
template <typename T>
std::ostream& operator<< (std::ostream& out, const std::vector<T>& v) {
if (!v.empty()) {
out << '[';
std::copy(v.begin(), v.end(), std::ostream_iterator<T>(out, ", "));
out << "\b\b]";
}
return out;
}
int num = 16;
//相机中组标定板的位姿，x,y,z,w,x,y,z
Mat_<double> CalPose = (cv::Mat_<double>(num, 7) <<
-0.019142697,0.008801379 ,0.211938128, -0.030625263 ,-0.01605667, 0.998521559, 0.041940109,//1
-0.018183947 ,-0.01305026 ,0.266784608 ,-0.0305709842782, -0.01733744 ,0.990492948 ,0.1329983,//2
0.052254666, 0.003839179 ,0.229174137, -0.0124332456699 ,0.097019154, 0.987631484, 0.122542847,//3
-0.00218128, 0.040340081 ,0.280634135 ,-0.0173363 ,-0.006932675, 0.999253326 ,-0.033825736,//4
0.00097711 ,0.044277284 ,0.268945873, 0.027248854 ,0.008675707 ,0.998468688, -0.047355149,//5
0.070512287 ,-0.029839678 ,0.245356679 ,0.00587619991395, 0.009445105,0.999025508 ,0.042711756,//6
0.01260369 ,-0.029761016, 0.244636059, 0.112973851 ,-0.054361917, 0.991293555, 0.040234058,//7
-0.019118361, -0.017231764 ,0.233383402 ,-0.106525247 ,0.410003692 ,0.896736898 ,0.128110422,//10
0.099970877 ,0.019913295, 0.27814135 ,-0.171188348, -0.026357483, 0.977001287 ,0.124371695,//11
0.066922761 ,0.033534456 ,0.246380225 ,-0.035780141, 0.095604627, 0.99024207 ,0.094869276,//12
0.008322123, 0.002318738, 0.325623274, -0.023902414, -0.110716933, 0.972771369 ,0.20220361,//13
0.024424298 ,0.021282304 ,0.297789633 ,-0.028250203, -0.018387057 ,0.997930176 ,0.054765001,//14
0.038527615 ,0.011775037 ,0.287405342 ,0.086648789 ,-0.008709179, 0.995841732, 0.026746618,//15
0.003589922,0.00701269,0.258110791, 0.0226302680186 ,-0.030048007,0.998662633,0.03546735,//16
0.063243598,0.017215444,0.242752165,-0.172558339,0.016110283,0.983767444,0.046537029,//17
0.103742972,0.031597719,0.322632223,0.138930386, -0.017062396,0.989923597,0.021412465);
//机械臂末端的位姿，x,y,z,w,x,y,z
Mat_<double> ToolPose = (cv::Mat_<double>(num, 7) <<
0.080115429, 0.188752756, 0.268797095, 0.711122423 ,-2.89506E-05, -1.76296E-05 ,0.703068204,
0.079509091, 0.135887183 ,0.307017162 ,0.708490925 ,-0.060471851, 0.061237559, 0.700452515,
0.049454675, 0.136262666, 0.306985693, 0.725050330725, -0.061949952, 0.059867487, 0.683286254,
0.049421024, 0.128409149 ,0.251500415 ,0.725792606, 0.041424009 ,-0.049814442, 0.684855945,
0.02010073, 0.130137971,0.25125298, 0.753329021, 0.043344859 ,-0.048035788 ,0.654453338,
0.0199721, 0.129670255 ,0.22324334 ,0.754805574 ,-0.004757904 ,0.007351554 ,0.655890129,
-0.024412855 ,0.135925483 ,0.223350168, 0.813153915, -0.005418092 ,0.006905753 ,0.581982531,
// -0.040225057, 0.141633432, 0.240963427, 0.853272398, -0.005880946, 0.006437633, 0.521392545
// 0.073773508, 0.123675173 ,0.240700209, 0.718363659 ,-0.004314918, 0.007587391, 0.695613015
0.112837801 ,0.122723284 ,0.259375359, 0.615747852 ,-0.048330788 ,0.043396404, 0.785261402,
0.052193137, 0.117061429, 0.32842347, 0.614743163 ,-0.060138307, 0.028169535, 0.785926654,
0.052202149, 0.117077284, 0.32842138, 0.715579296, -0.063443094 ,0.019756814 ,0.695363872,
0.046541136, 0.078699691 ,0.402184185 ,0.708776168 ,-0.141292184 ,0.100289471 ,0.683823723,
0.052306526, 0.0556187, 0.302499524, 0.715848464, -0.039502601 ,-0.004856761, 0.697120458,
0.012201325 ,0.099124405, 0.261528262, 0.79822923, 2.60876E-05, -1.68821E-05, 0.602353795,
0.022673847, 0.13587605, 0.261526637 ,0.752695169 ,2.43867E-06 ,3.71625E-05 ,0.658369183,
0.122156664, 0.122508, 0.26153452, 0.614337728 ,-2.13706E-05 ,-3.35018E-05, 0.789043189,
-0.088553092 ,0.128898855, 0.274293529, 0.831408557, -1.5297E-05 ,-4.24533E-05 ,0.555661596);
// -0.085495239 0.136304549 0.363611561 0.848087503 -0.061449431 0.084416486 0.519466469
// 0.021827131 0.094066761 0.334487546 0.779167445 -0.050555801 0.090946723 0.618118838
//R和T转RT矩阵
Mat R_T2RT(Mat& R, Mat& T)
{
Mat RT;
Mat_<double> R1 = (cv::Mat_<double>(4, 3) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
0.0, 0.0, 0.0);
cv::Mat_<double> T1 = (cv::Mat_<double>(4, 1) << T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0), 1.0);
cv::hconcat(R1, T1, RT);//C=A+B左右拼接
return RT;
}
//RT转R和T矩阵
void RT2R_T(Mat& RT, Mat& R, Mat& T)
{
cv::Rect R_rect(0, 0, 3, 3);
cv::Rect T_rect(3, 0, 1, 3);
R = RT(R_rect);
T = RT(T_rect);
}
//判断是否为旋转矩阵
bool isRotationMatrix(const cv::Mat& R)
{
cv::Mat tmp33 = R({ 0,0,3,3 });
cv::Mat shouldBeIdentity;
shouldBeIdentity = tmp33.t() * tmp33;
cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());
return cv::norm(I, shouldBeIdentity) < 1e-6;
}
/** @brief 欧拉角 -> 3*3 的R
* @param eulerAngle 角度值
* @param seq 指定欧拉角xyz的排列顺序如："xyz" "zyx"
*/
cv::Mat eulerAngleToRotatedMatrix(const cv::Mat& eulerAngle, const std::string& seq)
{
CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);
eulerAngle /= 180 / CV_PI;
cv::Matx13d m(eulerAngle);
auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
auto xs = std::sin(rx), xc = std::cos(rx);
auto ys = std::sin(ry), yc = std::cos(ry);
auto zs = std::sin(rz), zc = std::cos(rz);
cv::Mat rotX = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, xc, -xs, 0, xs, xc);
cv::Mat rotY = (cv::Mat_<double>(3, 3) << yc, 0, ys, 0, 1, 0, -ys, 0, yc);
cv::Mat rotZ = (cv::Mat_<double>(3, 3) << zc, -zs, 0, zs, zc, 0, 0, 0, 1);
cv::Mat rotMat;
if (seq == "zyx") rotMat = rotX * rotY * rotZ;
else if (seq == "yzx") rotMat = rotX * rotZ * rotY;
else if (seq == "zxy") rotMat = rotY * rotX * rotZ;
else if (seq == "xzy") rotMat = rotY * rotZ * rotX;
else if (seq == "yxz") rotMat = rotZ * rotX * rotY;
else if (seq == "xyz") rotMat = rotZ * rotY * rotX;
else {
cv::error(cv::Error::StsAssert, "Euler angle sequence string is wrong.",
__FUNCTION__, __FILE__, __LINE__);
}
if (!isRotationMatrix(rotMat)) {
cv::error(cv::Error::StsAssert, "Euler angle can not convert to rotated matrix",
__FUNCTION__, __FILE__, __LINE__);
}
return rotMat;
//cout << isRotationMatrix(rotMat) << endl;
}
/** @brief 四元数转旋转矩阵
* @note 数据类型double； 四元数定义 q = w + x*i + y*j + z*k
* @param q 四元数输入{w,x,y,z}向量
* @return 返回旋转矩阵3*3
*/
cv::Mat quaternionToRotatedMatrix(const cv::Vec4d& q)
{
double w = q[0], x = q[1], y = q[2], z = q[3];
double x2 = x * x, y2 = y * y, z2 = z * z;
double xy = x * y, xz = x * z, yz = y * z;
double wx = w * x, wy = w * y, wz = w * z;
cv::Matx33d res{
1 - 2 * (y2 + z2), 2 * (xy - wz), 2 * (xz + wy),
2 * (xy + wz), 1 - 2 * (x2 + z2), 2 * (yz - wx),
2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (x2 + y2),
};
return cv::Mat(res);
}
/** @brief ((四元数||欧拉角||旋转向量) && 转移向量) -> 4*4 的Rt
* @param m 1*6 || 1*7的矩阵 -> 6 {x,y,z, rx,ry,rz} 7 {x,y,z, qw,qx,qy,qz}
* @param useQuaternion 如果是1*7的矩阵，判断是否使用四元数计算旋转矩阵
* @param seq 如果通过欧拉角计算旋转矩阵，需要指定欧拉角xyz的排列顺序如："xyz" "zyx" 为空表示旋转向量
*/
cv::Mat attitudeVectorToMatrix(cv::Mat m, bool useQuaternion, const std::string& seq)
{
CV_Assert(m.total() == 6 || m.total() == 7);
if (m.cols == 1)
m = m.t();
cv::Mat tmp = cv::Mat::eye(4, 4, CV_64FC1);
//如果使用四元数转换成旋转矩阵则读取m矩阵的第第四个成员，读4个数据
if (useQuaternion) // normalized vector, its norm should be 1.
{
cv::Vec4d quaternionVec = m({ 3, 0, 4, 1 });
quaternionToRotatedMatrix(quaternionVec).copyTo(tmp({ 0, 0, 3, 3 }));
// cout << norm(quaternionVec) << endl;
}
else
{
cv::Mat rotVec;
if (m.total() == 6)
rotVec = m({ 3, 0, 3, 1 }); //6
else
rotVec = m({ 7, 0, 3, 1 }); //10
//如果seq为空表示传入的是旋转向量，否则"xyz"的组合表示欧拉角
if (0 == seq.compare(""))
cv::Rodrigues(rotVec, tmp({ 0, 0, 3, 3 }));
else
eulerAngleToRotatedMatrix(rotVec, seq).copyTo(tmp({ 0, 0, 3, 3 }));
}
tmp({ 3, 0, 1, 3 }) = m({ 0, 0, 3, 1 }).t();
//std::swap(m,tmp);
return tmp;
}
int main()
{
//定义手眼标定矩阵
std::vector<Mat> R_gripper2base;
std::vector<Mat> t_gripper2base;
std::vector<Mat> R_target2cam;
std::vector<Mat> t_target2cam;
Mat R_cam2gripper = (Mat_<double>(3, 3));
Mat t_cam2gripper = (Mat_<double>(3, 1));
vector<Mat> images;
size_t num_images = num;
// 读取末端，标定板的姿态矩阵 4*4
std::vector<cv::Mat> vecHb, vecHc;
cv::Mat Hcb;//定义相机camera到末端grab的位姿矩阵
Mat tempR, tempT;
for (size_t i = 0; i < num_images; i++)//计算标定板位姿
{
cv::Mat tmp = attitudeVectorToMatrix(CalPose.row(i), true, ""); //转移向量转旋转矩阵
vecHc.push_back(tmp);
RT2R_T(tmp, tempR, tempT);
R_target2cam.push_back(tempR);
t_target2cam.push_back(tempT);
}
//cout << R_target2cam << endl;
for (size_t i = 0; i < num_images; i++)//计算机械臂位姿
{
//cv::Mat tmp = attitudeVectorToMatrix(ToolPose.row(i), true, "zyx");
cv::Mat tmp = attitudeVectorToMatrix(ToolPose.row(i), true, ""); //机械臂位姿为欧拉角-旋转矩阵
//tmp = tmp.inv();//机械臂基座位姿为欧拉角-旋转矩阵
vecHb.push_back(tmp);
RT2R_T(tmp, tempR, tempT);
R_gripper2base.push_back(tempR);
t_gripper2base.push_back(tempT);
}
//cout << t_gripper2base[0] << " " << t_gripper2base[1] << " " << t_gripper2base[2] << " " << endl;
//cout << t_gripper2base << endl;
//手眼标定，CALIB_HAND_EYE_TSAI法为11ms，最快
calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper, CALIB_HAND_EYE_TSAI);
Hcb = R_T2RT(R_cam2gripper, t_cam2gripper);//矩阵合并
std::cout << "Hcb 矩阵为： " << std::endl;
std::cout << Hcb << std::endl;
cout << "是否为旋转矩阵：" << isRotationMatrix(Hcb) << std::endl << std::endl;//判断是否为旋转矩阵
//Tool_In_Base*Hcg*Cal_In_Cam，用第一组和第二组进行对比验证
cout << "第一组和第二组手眼数据验证：" << endl;
cout << vecHb[0] * Hcb * vecHc[0] << endl << vecHb[1] * Hcb * vecHc[1] << endl << endl;//.inv()
cout << "标定板在相机中的位姿：" << endl;
cout << vecHc[1] << endl;
cout << "手眼系统反演的位姿为：" << endl;
//用手眼系统预测第一组数据中标定板相对相机的位姿，是否与vecHc[1]相同
cout << Hcb.inv() * vecHb[1].inv() * vecHb[0] * Hcb * vecHc[0] << endl << endl;
cout << "----手眼系统测试----" << endl;
cout << "机械臂下标定板XYZ为：" << endl;
for (int i = 0; i < vecHc.size(); ++i)
{
cv::Mat cheesePos{ 0.0,0.0,0.0,1.0 };//4*1矩阵，单独求机械臂下，标定板的xyz
cv::Mat worldPos = vecHb[i] * Hcb * vecHc[i] * cheesePos;
cout << i << ": " << worldPos.t() << endl;
}
getchar();
}
然后新建CMakeLists.txt文件
vim CMakeLists.txt
# cmake needs this line
cmake_minimum_required(VERSION 3.1)
# Define project name
project(opencv_example_project)
# Find OpenCV, you may need to set OpenCV_DIR variable
# to the absolute path to the directory containing OpenCVConfig.cmake file
# via the command line or GUI
find_package(OpenCV REQUIRED)
# If the package has been found, several variables will
# be set, you can find the full list with descriptions
# in the OpenCVConfig.cmake file.
# Print some message showing some of them
message(STATUS "OpenCV library status:")
message(STATUS " config: ${OpenCV_DIR}")
message(STATUS " version: ${OpenCV_VERSION}")
message(STATUS " libraries: ${OpenCV_LIBS}")
message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}")
# Declare the executable target built from your sources
add_executable(opencv_example eyehand_cul.cpp)
# Link your application with OpenCV libraries
target_link_libraries(opencv_example PRIVATE ${OpenCV_LIBS})
然后编译
mkdir build && cd build
cmake ..
make
./opencv_example
输出如下
Hcb 矩阵为：
[0.2529948966039119, 0.653276241650712, 0.7135991412460475, 0.03017181911674481;
-0.9616527451985686, 0.2505542525409881, 0.1115641707076408, -0.001168880560819843;
-0.1059130773060283, -0.7144597389834835, 0.6916138383716883, -0.07547482289580558;
0, 0, 0, 1]
是否为旋转矩阵：1
第一组和第二组手眼数据验证：
[-0.9599436488593028, -0.2792873236999738, 0.02251185762079012, 0.03910078804958135;
-0.2214110370594403, 0.7068656080347647, -0.6718021783806323, 0.3715502367630538;
0.171712974154593, -0.6498766069605237, -0.7403884453808385, 0.3356351522593363;
0, 0, 0, 1]
[-0.9571525726204432, -0.2881555801842336, -0.02872826729728267, 0.03927764326193531;
-0.195760970020495, 0.7169541282410056, -0.6690698169418562, 0.361339517428391;
0.2133930511483864, -0.634778023840171, -0.7426441043317652, 0.3899559117828131;
0, 0, 0, 1]
标定板在相机中的位姿：
[-0.9975296556812412, -0.02621344623609158, -0.06517238877465595, -0.018183947;
-0.04247700198940089, 0.9640217307427128, 0.2624077112806484, -0.01305026;
0.05594898858924794, 0.2645278017033053, -0.9627537337269685, 0.266784608;
0, 0, 0, 1]
手眼系统反演的位姿为：
[-0.9997047217560895, -0.01765094882544278, -0.01670075395733612, -0.008077176050000887;
-0.02122639774834765, 0.9688851305323734, 0.2465989550282803, 0.03666166641663988;
0.0118284064209383, 0.2468806374594315, -0.9689737049477073, 0.2448389356423604;
0, 0, 0, 1]
----手眼系统测试----
机械臂下标定板XYZ为：
0: [0.03910078804958135, 0.3715502367630538, 0.3356351522593363, 1]
1: [0.03927764326193531, 0.361339517428391, 0.3899559117828131, 1]
2: [0.08683425441662714, 0.3536004451954924, 0.344910525909183, 1]
3: [0.02034896224340062, 0.3730021028587656, 0.3734935601188322, 1]
4: [0.01512751433860177, 0.3721913333345857, 0.3619222720564731, 1]
5: [0.09750146300366933, 0.3262202586990512, 0.3276439530655735, 1]
6: [0.03073768486914086, 0.3183407005095492, 0.3336689835478626, 1]
7: [0.02995544030800457, 0.3001712206250036, 0.3357754570798158, 1]
8: [0.04285151011411379, 0.3998638437274073, 0.387024726387165, 1]
9: [0.08581249543011772, 0.3669269088202225, 0.3650878968232826, 1]
10: [0.02323560666223969, 0.3795163686181222, 0.4495181615632068, 1]
11: [0.03880720012554556, 0.3239243677987539, 0.401306202842833, 1]
12: [0.08461018689300819, 0.3412465124182371, 0.3723482669356442, 1]
13: [0.02624041046694395, 0.3572248296625789, 0.3591640752105018, 1]
14: [0.0952593972169479, 0.3536129148496938, 0.334956523944783, 1]
15: [0.08165612719991988, 0.3910115315510737, 0.3884131893555852, 1]
参考网站
RM机械臂与Realsense D435手眼标定教程 - 知乎 (zhihu.com)
（已修正精度 1mm左右）Realsense d435i深度相机+Aruco+棋盘格+OpenCV手眼标定全过程记录_realsense相机标定_Fr0mdeepsea的博客-CSDN博客
机械臂urdf模型修改及功能包配置_stl转urdf-CSDN博客
sunday_description · master · mirrors / lord-z / sunday · GitCode
从零开始的机械臂yolov5抓取gazebo仿真（一）_Lord_ZYX的博客-CSDN博客
1935
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
