UFactory xArm6 的xarm_ros开发
背景
最近要对机械臂进行开发控制,部分需求是:机器人导航-摄像头目标检测-3D坐标输入到机械臂-控制机械臂并控制末端喷头消毒
环境部署
-
当然是先看官网
-
直接安装xArmStudio-linux-0.6.0.zip,输入机械臂默认的IP地址使能后就能控制
![在这里插入图片描述](https://img-blog.csdnimg.cn/ef8ce1c732904d008003007a1ecc7b6a.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_20,color_FFFFFF,t_70,g_se,x_16#pic_center)
-
按照官网流程安装如下功能包
gazebo_ros_pkgs
ros_control
moveit_core
xarm_ros
其他依赖包
以上就安装完啦
简单的跑一下,模拟三维坐标输入来控制机械臂移动。
终端1启动xarm节点
roslaunch xarm_bringup xarm6_server.launch robot_ip:=192.168.1.214
终端2
rosservice list
输出
/rosout/get_loggers
/rosout/set_logger_level
/xarm/clear_err
/xarm/config_tool_modbus
/xarm/get_analog_in
/xarm/get_controller_ain
/xarm/get_controller_din
/xarm/get_digital_in
/xarm/get_err
/xarm/get_position_axis_angle
/xarm/get_position_rpy
/xarm/get_servo_angle
/xarm/get_tgpio_modbus_baudrate
/xarm/go_home
/xarm/gripper_config
/xarm/gripper_move
/xarm/gripper_state
/xarm/motion_ctrl
/xarm/move_joint
/xarm/move_jointb
/xarm/move_line
/xarm/move_line_aa
/xarm/move_line_tool
/xarm/move_lineb
/xarm/move_servo_cart
/xarm/move_servo_cart_aa
/xarm/move_servoj
/xarm/moveit_clear_err
/xarm/play_traj
/xarm/save_traj
/xarm/set_collision_rebound
/xarm/set_collision_sensitivity
/xarm/set_controller_aout
/xarm/set_controller_dout
/xarm/set_digital_out
/xarm/set_fence_mode
/xarm/set_joint_jerk
/xarm/set_load
/xarm/set_max_acc_joint
/xarm/set_max_acc_line
/xarm/set_mode
/xarm/set_recording
/xarm/set_reduced_mode
/xarm/set_state
/xarm/set_tcp_jerk
/xarm/set_tcp_offset
/xarm/set_teach_sensitivity
/xarm/set_tool_modbus
/xarm/set_world_offset
/xarm/vacuum_gripper_set
/xarm/velo_move_joint
/xarm/velo_move_joint_timed
/xarm/velo_move_line
/xarm/velo_move_line_timed
/xarm/xarm_driver/get_loggers
/xarm/xarm_driver/set_logger_level
机械臂线性移动,这个可以先手动模式移动机械臂到固定位置,记录下x,y,z,Roll,pitch,yaw数值。执行rosservice type /xarm/move_line
和rossrv show xarm_msgs/Move
可以查到这个pose[]是float32[]类型,而且最后三个最后要转换成rad。线速度200和加速度2000,当时调到了20和30。因为太快不行,后面再考虑怎么加快机械臂移动。
rosservice call /xarm/move_line [250,100,300,3.14,0,0] 200 2000 0 0
回到零点
rosservice call /xarm/go_home [] 0.35 7 0 0
至于movelt包也运行过,可以仿真和同时运行真实的机械臂
相机D435i标定
步骤是:配置环境-RGB标定-imu_utils的IMU标定-双目标定-双目IMU联合标定
我的理解是:对RGB摄像头标定只是为了单目标定而已并没什么意义,然后再对两个双目红外摄像头标定,然后IMU标定,最后将双目红外和imu联合标定
我是按照这位博主链接一步步来:步骤链接
最终标定完,可通过results-imucam-homeicrcalib_datamii-d435imulti_camera_caliimu_stereo.txt,看到imu0 to cam0,cam0 to imu0,imu0 to cam1,cam1 to imu0的转换矩阵T_ic,如下图所示。
![在这里插入图片描述](https://img-blog.csdnimg.cn/12b744f61c7b4c559872396c56f79333.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_20,color_FFFFFF,t_70,g_se,x_16)
最后:双目和imu联合标定后得到相机的内外参,内参用来纠正畸变,外参用来坐标系转换(机械臂和摄像头联合标定时候要用到)
虽然我整个过程标定完了,但我有几个疑问:1.如何判断标定误差多大 2.最后的双目加imu联合标定是去掉rgb的内参来标定的,所以我如果要用rgb摄像头执行yolo检测物体并且用机械臂进行控制的话,那么我是以rgb摄像头为坐标系吗?这些问题等手眼标定后再来思考。
相机标定遇到的错误:
- 期间会遇到 pyx模块没有安装,apt安装即可
sudo apt-get install python-pyx
![在这里插入图片描述](https://img-blog.csdnimg.cn/5e18b39b3682421aa8a0ecc8996ef019.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_20,color_FFFFFF,t_70,g_se,x_16)
Cameras are not connected through mutual observations, please check the data
解决:修改aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py
def isGraphConnected(self):
#check if all vertices are connected
#return self.G.adhesion()
if self.numCams == 1:
# Since igaph 0.8, adhesion correctly returns 0 for the non-connected one cam case.
# which evaluates to false later on. So we skip the check and return true in the one camera case.
return True
else:
#check if all vertices are connected
return self.G.adhesion()
[ERROR] Did not converge in maxIterations... restarting...
解决:
可能性一:拍摄的时候,角度不要变化太大,不然会初始化失败并且优化发散。因为初始化的时候就必须要得到一个较好的估计,但如果有太大角度的图片,很容易得到错误的初值,所以后面就会优化失败。
可能性二:标定板的yaml的宽列是否反了。我下面的是8列6行
target_type: 'checkerboard' #gridtype
targetCols: 8 #number of internal chessboard corners
targetRows: 6 #number of internal chessboard corners
rowSpacingMeters: 0.0245 #size of one chessboard square [m]
colSpacingMeters: 0.0245 #size of one chessboard square [m]
手眼标定
按官网教程安装功能包
xarm_ros(已装)
ros(已装)
aruco_ros
easy_handeye
vision_visp
find_object_2d
其他依赖包
全部安装到一个工作空间下,用catkin build 并行编译,方便catkin clean后git上传。但期间遇到一个问题就是D435i摄像头在接入usb3.0后只能使用realsense的SDK包realsense2_camera而不能使用usb_cam或者uvc_cam功能包。(简单来说有三种方式调用摄像头,第一种就是realsense的SDK驱动,第二种是用usb的驱动,第三种是用uvc驱动)。这个问题真的不知道是供电导致呢还是摄像头参数配置有问题,先暂时放放。
手眼标定前先检测aruco标签是否正常
ArUco_ros的使用:
原理介绍:
aruco采用2D图像计算深度信息,需要提前在程序中输入标记物的宽度,根据识别标记物在图像中的像素数量与实际宽度的比值即可计算出深度等信息。
先到网站打印标签
Dictionary 一定要选 Original ArUcoMarker ID 和 Marker size 自选,在launch
文件中做相应的修改打印时,要选择原始大小,否则要测量一下打印出来的真实大小
![在这里插入图片描述](https://img-blog.csdnimg.cn/db4c944cb92649dfb40d57b7f9bdb2a0.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_16,color_FFFFFF,t_70,g_se,x_16)
修改aruco_ros/aruco_ros/launch/single.launch,因为我用的是D435i,修改对应的camera_info和image主题即可。以及修改markerId、markerSize
<launch>
<arg name="markerId" default="3"/>
<arg name="markerSize" default="0.01"/> <!-- 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 -->
<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="stereo_gazebo_$(arg eye)_camera_optical_frame"/>
<param name="marker_frame" value="$(arg marker_frame)" />
<param name="corner_refinement" value="$(arg corner_refinement)" />
</node>
</launch>
执行如下代码
roslaunch realsense2_camera rs_camera.launch
roslaunch aruco_ros single.launch
rqt_image_view
rosrun image_view image_view image:=/aruco_single/result
rostopic echo /aruco_single/pose
如图显示检测到的标签以及输出的xyz坐标
![在这里插入图片描述](https://img-blog.csdnimg.cn/718122e92a36423692b0cfb84c243fe6.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_20,color_FFFFFF,t_70,g_se,x_16)
UFactory xArm6 和 D435i的Eye-to-hand 眼在手外标定
眼在手外获得的是相机与机械臂基座标坐标变换,而眼在手上获得的是相机与机械臂末端(工具坐标系)坐标变换。
参考网址
遇到问题1:module’ has no attribute ‘CALIB_HAND_EYE_TSAI
错误文件地方改为:
import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
import cv2
sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
遇到问题2:依然报错,无法弹出三个标定窗口
问了ufactory厂家,厂家说没有视觉的支持,那就只能自己找easy_handle的标定流程了。
正常启动后是有三个窗口:
机械臂的RViz窗口
外参矩阵计算窗口
手眼标定操作窗口
问题是我卡了一天了,只有机械臂的rviz窗口显示,别人都说直接就出来三个窗口,我也是醉了。到底是哪里配置错了-_-。
最终在别人的手眼标定评论里看到别人说不出现三个窗口可以执行下面指令:
python -m pip install opencv-contrib-python
漂亮!虽然launch启动还有错,但已经可以看到rqt_easy_handle__Hand-eye Calibration-rqt的界面啦。
tip:(执行可能不成功,说python2.7没安装pip)
![在这里插入图片描述](https://img-blog.csdnimg.cn/7e655c491b3f42a7bfb314b0855d0ed0.png)
则可以参考这位博主执行以下代码
curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
python get-pip.py
还是不能显示Hand-eye Calibration automatic movement ,然后看到有评论说是因为opencv的版本过低,导致没有手眼校准算法。因为目前我是用python2.7,python-opencv是3.4.1.于是又去下opencv4.2.0源码编译。折腾了一天,发现环境是这样的,ubuntu自带的python2.7,关联opencv2.4.9.而ros里的python2.7是关联opencv3.3.1-dev,然后我一顿操作直接把ros的cv2.so删了之后,绑定了源码下载的 opencv4.2.0了,重新运行每个节点,都没有报错,但发现还是无法出现“Hand-eye Calibration automatic movement”这个ui界面。我崩溃了啊。之后看plugin.xml发现手眼标定操作窗口就是无法启动.
我崩溃了啊,好无语,有这么难搞的吗,难道升级了18.04就能解决问题?慢慢再摸索发现自己的launch配置有问题,现在列出我分开写的两个可以运行的launch。
![在这里插入图片描述](https://img-blog.csdnimg.cn/3c8591d6977a4e1fafedda4731bebbf2.png)
就是红色这个框这个false,我写了true,导致不能启动手眼标定操作窗口
![在这里插入图片描述](https://img-blog.csdnimg.cn/f17548523c2f4969b766014caa21d1ed.png)
然后我强制运行,直接rosrun rqt_easy_handeye rqt_calibrationmovements
,好的能看到了。
![在这里插入图片描述](https://img-blog.csdnimg.cn/e6af093571a142cfbf41bb11e2b2cc8c.png)
但是呢,我在对准aruco后点击"check starting pose" 就报错,如下图所示
![在这里插入图片描述](https://img-blog.csdnimg.cn/e8c4fcf61d444c64972847561a606144.png)
之后用xarm_studio移动机械臂标定就好了,我17个点,只成功了12个。
![在这里插入图片描述](https://img-blog.csdnimg.cn/354038640e5a44c98e900f0978219c9c.png)
遇到问题3:rqt没有菜单栏,无法打开标签识别的视频流,正常Plugins->Visualization->Image View,但运行rqt后是一个空白窗口,没有菜单栏
解决方法如下:
其实ubuntu应用的菜单栏都是显示在最上面任务栏的,不管应用窗口是不是最大化,所以直接Plugins->Visualization->Image View->选topic
也可以rviz->add image->选topic显示
也可以rosrun image_view image_view image:=/aruco_tracker/result
也可以launch里面添加
![在这里插入图片描述](https://img-blog.csdnimg.cn/c083c860c8df465598dbe50119259629.png)
easy_handle的publish.py是将标定好的 TF 发布出去的。修改了如下三个地方。
![在这里插入图片描述](https://img-blog.csdnimg.cn/f600eaddae1b4c97b5829e32e8bce873.png)
可以看看基座标系是否有连接上
rosrun rqt_tf_tree rqt_tf_tree
![在这里插入图片描述](https://img-blog.csdnimg.cn/7c030057f43947db8fb2d62ed7417f82.png)
find_object_2d or find_object_3d
参考链接1
参考链接2
效果不是很好,当角度过大会导致识别标签失败
遇到的问题
- 问题1:安装movelt步骤中cmake build 报错:
![在这里插入图片描述](https://img-blog.csdnimg.cn/09ae45c8291b4a5ea372fc29c13c8ed5.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_20,color_FFFFFF,t_70,g_se,x_16)
解决方法:
看rosdep install -y --from-paths . --ignore-src --rosdistro kinetic
后缺少什么依赖(可以参考这篇:文章)
![在这里插入图片描述](https://img-blog.csdnimg.cn/c84e90a4d88745bea27080cc2216b747.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA57uR5Liq6J206J2257uT,size_20,color_FFFFFF,t_70,g_se,x_16)
然后安装每一条依赖:eg: sudo apt-get install ros-kinetic-tf-conversions