UR5+robotiq_85_gripper GAZEBO模拟视觉抓取平台仿真-2

6,视觉算法

这里我以OpenCv中的SIFT描述子检测目标物体相对于相机的位姿。这里有个开源的包。find_object_2d是ROS平台下一个功能强大的识别物体的功能包。由于对于纯颜色物体具有很少的特征，所以我决定把另外两个物快设置成纯红色，只有一个物快设置成木质色用于目标抓取。

1,首先把之前平台搭建起的Kinect V2图像的RGB和深度图像作为输入进行包的重映射。
find_object_3d_kinect2.launch

		<remap from="rgb/image_rect_color" to="/kinect_V2/rgb/image_raw"/>
		<remap from="depth_registered/image_raw" to="/kinect_V2/depth/image_raw"/>
		<remap from="depth_registered/camera_info" to="/kinect_V2/depth/camera_info"/>

2,然后需要设置SIFT参数


3,截图选定目标物体区域

这块的设置步骤大致是这样，下一步就是结合之前的模拟环境进行视觉抓取
1,

roslaunch ur5_single_arm_tufts ur5_single_arm_gazebo.launch

这里要强调一下，这里木色物快的位姿相对于机械臂基座标的位姿。
（0.6 0.0 0.181 0 0 0 ）

2,

roslaunch find_object_2d find_object_3d_kinect2.launch

按照上面的配置提示，可以识别物体的位姿，并发布到TF树中，用于订阅抓取

启动RVIZ的TF树，可以查看发布的物体坐标，这里有个疑问在于发布的物体坐标在姿态上有变换


我也没搞明白为什么，先留下一个疑问吧。
可以运行

rqt_tf_tree rqt_tf_tree

形象的查看tf树的关联

可以物体坐标是可以通过kinect v2的坐标转换到world（base_link）坐标下,我的模型两者的坐标系重合。

3,

rosrun opencv tf_listener.py

需要注意程序中是根据rviz下的物体序号进行修改

这个函数主要是订阅tf树中的world和object的坐标变换，然后进行重新话题发布物体位姿到/objection_position_pose 话题下

这里需要对比我们之前设定好的位姿法线，在位置上的误差并不是很大，但是姿态的变化还存在疑问。
（0.6 0.0 0.181）
（0.5840022262580775, 0.057023499981076364, 0.18070173712197124）
4,

rosrun ur5_single_arm_manipulation grasping_demo_vision2.py 

主要是一套抓取路点结合，但是其中物体的位姿是订阅话题/objection_position_pose，主要通过一个callback回调函数进行了路点设定，部分程序如下

def callback(pose):
    object_position_info = pose.position
    object_orientation_info = pose.orientation
    print object_position_info

    moveit_commander.roscpp_initialize(sys.argv)
    #rospy.init_node('move_group_grasp', anonymous=True)
    #robot = moveit_commander.robot.RobotCommander()

    arm_group = moveit_commander.move_group.MoveGroupCommander("manipulator")
    hand_group = moveit_commander.move_group.MoveGroupCommander("gripper") 
   
    arm_group.set_named_target("home_j")
    plan = arm_group.go()

    print("Point 1")

    # Open
    #hand_group.set_joint_value_target([9.800441184282249e-05, -9.800441184282249e-05,   9.800441184282249e-05, 9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05])
    #hand_group.go(wait=True)
    #print("Point 2")
    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("Point 2")

    pose_target = arm_group.get_current_pose().pose

    # Block point top
    pose_target.position.x = object_position_info.x
    pose_target.position.y = object_position_info.y
    pose_target.position.z = object_position_info.z+0.25

实验效果：

参考网址：
1,https://github.com/Dzy-HW-XD/kinectv2_ur5
2,https://github.com/ldahee/wecar/tree/1a6e5ebc7534f314410f1ee50a71c75df9a52c0b/find-object
3,http://introlab.github.io/find-object/
4,http://wiki.ros.org/find_object_2d

7,手眼标定

手眼标定的目的是为了得到相机相对于机械臂基座标的坐标变换。

主要程序链接：

https://github.com/harrycomeon/ur5-gazebo-visual-grasping/tree/master