Moveit!入门——古月居机械臂开发笔记（二）

引言

接上次笔记（一）Moveit！和机械臂控制，这一次主要为：ROS机械臂开发_机器视觉与物体抓取。
 

ROS机械臂开发_机器视觉与物体抓取

 

1、ROS中的运动学插件

       参考https://www.guyuehome.com/1921
       在Moveit！setup assistant配置中会选择一个运动学求解器，用于运动学反解。

KDL：

       很多点求解不出来，求解时间过长，i7处理器，一个姿态求解时间大概在0.01s-0.1s。

TRAC_IK：
       求解速度和求解成功率均高于KDL。但是TRAC-IK也有问题，它是一种数值算法，每次求解得到的关节位置不一定相同。

       使用方法——ROS的软件源中已经集成了TRAC-IK的安装包，可以直接使用以下命令安装：

sudo apt-get install ros-melodic-trac-ik-kinematics-plugin

       然后修改机械臂MoveIt!配置功能包下的kinematics.yaml文件就可以使用啦：

 kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
 kinematics_solver_attempts: 3
 kinematics_solver_search_resolution: 0.005
 kinematics_solver_timeout: 0.05

       此时运行配置功能包中的demo.launch使用的就是TRAC_IK求解器了。

IKFAST
       IKFAST是一种基于解析算法的运动学插件，可以保证每次求解的一致性。相比KDL和TRAC-IK，IKFAST的安装过程就比较复杂了，不过就笔者的使用经验来讲，IKFAST的效果还是很推荐的，所以不妨一试，
       Moveit官方教程配置方法

 

2、Moveit！碰撞检测

 

规划场景

       这一块即指在场景中添加障碍物体，在规划轨迹时考虑避障。需要我们做的不多，只需要添加物体，规划是自动进行的。
注：这里我理解的添加障碍物体是在Moveit！中添加，让其知道环境中有这么一个物体。而gazebo是仿真世界，在gazebo中的模型Moveit！不会主动避障，即需要3D sensors来做实时避障。

       在上一篇笔记中提到的轨迹约束中的运动规划器，使用OMPL中的算法。OMPL (Open Motion Planning Library) 是一个开源的运动规划库，主要是执行随机规划器。MoveIt直接整合OMPL，使用其库里的运动规划器作为主要/默认的一套规划器。

       Moveit！内部实现方法——Moveit！中有一个重要的功能模块叫规划场景监听器。

这一块均参考https://www.guyuehome.com/17370

       可以通过RVIZ（Publish Scene）来往场景添加物体，也可以通过编程（Moveit官方教程中有），还可以用ROBOT 3D Sensors实时检测场景中的障碍物。

       同时Moveit！也可以做到将被抓取物体纳入避障规划。

 

3、Pick and Place

1. 创建抓取的目标物体（已知物体的位置）
2. 设置目标物体的放置位置
3. 生成抓取姿态（生成一系列姿态，姿态之间轻微变化，防止无法求解）

# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose,  [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
	self.gripper_pose_pub.publish(grasp.grasp_pose)
	rospy.sleep(0.2)

4. Pick：

# 追踪抓取是否成功， 以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直到成功或超过最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
	n_attempts += 1
	rospy.loginfo('Pick attempt：' + str(n_attempts))
	result = arm.pick(target_id, grasps) # 上层的接口，类似arm.plan, arm.go类似
	rospy.sleep(0.2)

5. Place：

# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
	result = None
	n_attempts = 0
	# 生成放置姿态
	places = self.make_places(place_pose)
	# 重复尝试放置，直到成功或超过最大尝试次数
	while result != MoveItErrorCodes.SUCCESS and  n_attempts < max_place_attempts:
		n_attempts += 1
		rospy.loginfo('Place attempt：' + str(n_attempts))
		for place in places:
			result = arm.place(target_id, place) # 上层的操作接口
			if result == MoveItErrorCodes.SUCCESS:
				break
		rospy.sleep(0.2)
	if result != MoveItErrorCodes.SUCCESS:
		rospy.loginfo('Place operation failed after ' + str(n_attempts))

       在上面使用了非常上层的接口PICK/PLACE，其中具体怎么样去夹，怎么样去放，我们不知道其内部实现。

       这里有一个疑问是，Pick和Place都是对于机械臂本身的，并没有涉及夹具的open，close操作，那它是怎么完成抓取放置这些动作的呢？

 

4、ROS中的机器视觉

参考https://www.guyuehome.com/262?replytocom=49154
 

ROS中的图像数据

       图像数据消息类型如下图：

       针对压缩图像CompressedImage

       针对点云PointCloud2

摄像头标定

       摄像头这种精密仪器对光学器件的要求较高，由于摄像头内部与外部的一些原因，生成的物体图像往往会发生畸变，为了避免数据源造成的误差，需要针对摄像头的参数进行标定。ROS官方提供了用于双目和单目摄像头标定的功能包——camera_calibration。
ROS官方单目标定教程

或许可以拿笔记本自带摄像头试试内参标定（后面再试试）
这里提到的是内参标定，还需要外参标定，即眼在手上或眼在手外。
       安装标定功能包

sudo apt-get install ros-melodic-camera-calibration

       简要过程：

1. 启动摄像头，读取摄像头视频信息

	sudo apt-get install ros-melodic-usb-cam
	roslaunch usb_cam-test.launch
	rqt_image_view

       这个时候 调用的是你自己的笔记本上的摄像头，要去launch文件中把video0改成video1就可以调用外接摄像头啦！(一般默认笔记本自带摄像头是video0）

2. 启动标定程序，eg：

   rosrun camera_calibration cameracalibrator.py --size 11x8 --square 0.03 image:=/usb_cam/image_raw camera:=/usb_cam
   # size：标定棋盘格内部角点个数，这里使用的棋盘格一共有8行，每行11个
   # square：每个棋盘格的边长，单位米
   # image、camera：设置摄像头发布的图像话题
   

3. 如果是Kinect等深度相机，除了彩色摄像头还有红外深度摄像头，还需要进行一次标定
4. 摄像头使用标定结果文件(.yaml)，在启动摄像头的Launch文件中添加参数
    

ROS+OpenCV

sudo apt-get install ros-melodic-vision-opencv libopencv-dev python-opencv

       主要是通过CVBridge将ROS的图像数据转换成OpenCV的图像格式，进行图像处理后，再将OpenCV格式的图像数据转换成ROS图像消息，发布。
CVBridge官方教程：介绍了如何转换

       imgmsg_to_cv2()：将ROS图像消息转换成OpenCV的图像数据；
       cv2_to_imgmsg()：将OpenCV格式的图像数据转换成ROS图像消息；

ORK（Object Recognition Kitchen）

       ORK是ROS集成的物体识别库，其中有很多物体识别算法，如LineMod。
 

5、Moveit！视觉集成

       首先阐述一下我对这一块的理解，即它存在的意义。当在gazebo中仿真kinect模型时，可以获得仿真世界的RGBD信息，而这些信息Moveit！是不知道的，即在rviz界面是没有仿真物体的。这个时候可以添加相应的配置，利用Moveit！的场景监听器，将点云等数据传给Moveit！，使Moveit！将这些物体纳入路径规划。

       这一步应该是十分重要的，与此前我做的将机械臂移动到已知的指定位置完全不同。

       需要两步：

1. 修改Moveit配置包config文件夹中的sensors_3d.yaml如下

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
   - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
     point_cloud_topic: //kinect_V2/depth/points
     max_range: 5.0
     frame_subsample: 1
     point_subsample: 1
     padding_offset: 0.1
     padding_scale: 1.0
     max_update_rate: 1.0
     filtered_cloud_topic: filtered_cloud

2. 修改配置包launch文件夹中的ur5_moveit_sensor_manager.launch.xml修改如下：

<launch>
	<rosparam command="load" file="$(find ur5_single_arm_moveit_config)/config/sensors_3d.yaml" />
	<param name="octomap_frame" type="string" value="base_link" />
	<param name="octomap_resolution" type="double" value="0.04" />
	<param name="max_range" type="double" value="5.0" />
launch>

       其中第二个参数是你想要将八叉树地图发布在那个坐标系，第三个参数为方格的分辨率，最后一个为显示范围。

       这里有碰到一个奇怪的现象，设置octomap_resolution为0.05时，运行grasp_demo.py，机械臂到不了预定位置，设为0.04，0.03时又可以，初步猜测为0.05精度太低，在本应到达的抓取位置会与物体相碰，就没有执行到那步路径？

       同时由于kinect2把待抓取物体也纳入了障碍，导致要抓取的那一步规划出错。

       综合来看，要实现Pick&Place需要以下几步：

1. 摄像头标定（内参，外参）
2. 物体识别
3. 抓取姿态分析
4. 机械臂运动规划

       

后记

       在对Moveit和ros的使用流程有了基础认识之后，就要开始涉及整个完整的系统流程了，希望自己还能加油。想一想还有哪些没有完成：

1. 相机标定（内参，外参）
2. 一些坐标系之间的关系，还需要多实践加强认识
3. 结合视觉，识别物体，生成抓取姿态，完成pick&place
4. 以及最重要的如何将深度学习生成得到的抓取姿态与ros结合