¶ 卓尔-ARM视觉抓取
¶ 夹爪测试
夹爪开机前需处于闭合状态。首先启动真实机械臂通讯节点：
roslaunch lebai_lm3_moveit_config real_robot.launch
设置夹爪位置：
rosservice call /io_service/set_gripper_position "{val: 50}"
位置控制 val 值范围为 0-100，0 为完全闭合，100 完全张开。
设置夹爪力度：
rosservice call /io_service/set_gripper_force "{val: 10}"
力度 val 数值 0-100，可根据夹取的物体材质选择适当的力度。
一般来讲，我们只需要设置夹爪位置即可。
在进行实际的抓取之前，请先在空中测试夹爪的位置和力度。找到合适的值，增加抓取成功率。
¶ 手腕相机测试
在卓尔-ARM中，我们主要使用手腕相机来进行物体抓取，前面我们测试了车上相机，接下来来测试一下手腕相机的启动：
roslaunch astra_camera gemini_arm.launch
然后打开rqt_image_view测试功能正常：
rosrun rqt_image_view rqt_image_view
出现图像即为正常：
¶ 坐标变换
抓取之前，需要进行坐标变换，将末端工具坐标与手腕相机坐标位置进行变换发布。这里我们在真实机械臂启动的real_robot.launch中发布了此坐标变换。
<node pkg="tf" type="static_transform_publisher" name="hand_eye_result" args="0.117732 0.008468 -0.021280 -0.035549 0.686813 0.008200 0.725918 lebai_tool0 camera_link 100" />
¶ Aruco抓取
首先把标签打印好贴在硬纸板上或者贴在大小合适用于夹取的物品表面。标签的有效面积为黑色部分，但是需要保留一小部分白边，否则可能无法识别。
测量实际打印出的标签边长(黑色区域为有效边长)，在 grab_demo/launch/grab_ar.launch 文件中修改markerId 以及 markerSize，单位为米；
打印网址: https://chev.me/arucogen(Dictionary 选择 Original ArUco)。
修改完成后，打开终端输入：
roslaunch grab_demo grab_ar.launch
打开一个新的终端，输入：
rosrun grab_demo start_grab ar_pose_link
标签识别需要同时打开真实机械臂节点，机械臂相机节点，标签识别节点，动态 TF 坐标转换发布节点以及动态 TF 坐标转换订阅节点。
真实机械臂节点主要建立与机械臂的通讯以及发布相机于机械臂末端的静态 TF 位姿关系。
相机节点发布图像话题，标签识别节点订阅相机发布的图像话题进行识别并把目标二维码的位姿信息通过话题的形式发布，动态 TF 坐标转换发布节点订阅二维码的实时位姿信息并把它以TF坐标的形式广播出去.
其它的节点就可以直接从TF树获取二维码相对机械臂底座的位姿信息，最后通过机械臂运动学逆解到达目标点。
实时订阅二维码的位姿信息后，将位姿信息以 TF 坐标的形式连接到 TF 树中。
grab_demo/src/ar_pose_pub.cpp：
geometry_msgs::TransformStamped tfs; //创建广播的数据
tfs.header.frame_id = "camera_rgb_optical_frame"; //父坐标系
tfs.header.stamp = ros::Time::now();
tfs.child_frame_id = "ar_pose_link"; //创建的子坐标系
tfs.transform.translation.x = x; //设置坐标系相对父坐标系的位姿
tfs.transform.translation.y = y;
tfs.transform.translation.z = msg.pose.position.z;
tfs.transform.rotation.x = msg.pose.orientation.x;
tfs.transform.rotation.y = msg.pose.orientation.y;
grab_demo/src/start_grab.cpp：
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("base_link","ar_pose_link",ros::Time(0),ros::Duration(100));
target_pose.position.x=tfs.transform.translation.x;
target_pose.position.y=tfs.transform.translation.y;
target_pose.position.z=tfs.transform.translation.z;
lebai.moveP(target_pose.position.x,target_pose.position.y,target_pose.position.z);//夹爪抓取姿态为观察物体时的姿态
我们前面已经通过一系列的 TF 坐标转换，TF 树如图所示:
二维码坐标系->相机坐标系->机械臂末端坐标系->机械臂底座坐标系,调用 lookupTransform()实时监听机械臂底座坐标系与二维码坐标系的相对位姿，最后调用机械臂逆解将转换得到的二维码相对机械臂底座的位置 XYZ 与
姿态 rx ry rz 传入即可到达目标点，如夹取偏差，可通过优化相机内参以及手眼标定结果改善。