实际的机械臂控制（8）使用find_object3D和Kinect2实现目标跟踪(基于python)

单纯的炫耀我的新机械臂和留下联系方式

话不多说了。由于很多向入门机械臂的人不知道如何把视觉算法检测到目标坐标从图像坐标系转换到机器人坐标系。就这一关，让好多人包括我，在这块卡了很久。看到到这个博客的人，可以直接加我微信13304223197。不要打电话给我，仅微信联系，我无偿让你入门。后期我会考虑到作一期免费的教程。
以前我用的是小强机械臂，前面的博客有图像，现在有人赞助很多小钱钱，就买了一个真正的六自由度机械臂
就是这个，怎么样，很Diao吧。

在很多大佬的博客，主要是古月居的一些博客中，他们都介绍了使用find_object2D这个包是识别目标的位姿。但是如何将目标的位置和姿态发送给机械臂，他们都没有提及。这让我很尴尬呀，没人带入门，很生气，所以停止研究机械臂的控制，然后去继续搞视觉部分，一不小心发了个顶刊T-PAMI。
可能是借助于这个T-PAMI提供的元气，瞬间打通了我的任督二脉，让我瞬间-----------------------------------------------白瞟到了一个新的方法。
这个时在gazebo下面仿真的。

首先是安装find_object2D这个包，建议源码安装
具体安装细节参考这个教程。

关于如何将目标的位置发送给机械臂，目前大佬们不知道是藏私，还是不屑于讲，在他们的博客里少有提及。
目前说的最明确的是这个博客。
但是也只是说到了
将识别的坐标点发送给机械臂。原话如下：
在使用find_object_3d时，我们可以直接获得目标物体的tf坐标，因此可以使用ros自带的tf转换直接查询机械臂基座标到物体的tf关系，并发送给机械臂： 参考这里:http://docs.ros.org/lunar/api/tf/html/c++/classtf_1_1TransformListener.html http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29

tf::StampedTransform transform;
     try{
       listener.lookupTransform("/arm_base_link", "/object_1",  
                                ros::Time(0), transform);
     }
     catch (tf::TransformException ex){
       ROS_ERROR("%s",ex.what());
       ros::Duration(1.0).sleep();
     }

看到这便我也是糊里糊涂，感觉懂了，但是感觉有不懂，因为我还是无法让机械臂动起来。
下面开始我白票的方法。

首先是发送位姿

1. 发送坐标转换矩阵

大家使用和修改这个代码的时候，把里面的汉字注释掉。

#!/usr/bin/env python
import rospy
import tf
from geometry_msgs.msg import Pose
def object_position_pose(t,o):
    #话题名为： objection_position_pose，
    #后面订阅这个话题时要使用这个名字
    pub = rospy.Publisher('/objection_position_pose',Pose,queue_size=10)
    p = Pose()
    rate = rospy.Rate(5)
    p.position.x = t[0]
    p.position.y = t[1]
    p.position.z = t[2]

    p.orientation.x = o[0]
    p.orientation.y = o[1]
    p.orientation.z = o[2]
    p.orientation.w = o[3]
    pub.publish(p)
    rate.sleep()

if __name__ == '__main__':
   rospy.init_node('tf_listener',anonymous=True)#初始化一个节点
   listener = tf.TransformListener() 
   rate = rospy.Rate(10.0)
   while not rospy.is_shutdown():
   #主要需要修改的就是这个地方，前面的world是你的机械臂的世界坐标系，一般不要修改，后面的object_33是目标坐标系的名字，需要修改，这个名字需要与你在rviz下面看到的名字保持一致.看下图。
   #这一步就是将目标的坐标从目标坐标系转换到世界诶坐标系。注意这个(trans,rot) ，后面会用到，分别表示旋转和平移。
       try:
           (trans,rot) = listener.lookupTransform('/world', '/object_33', rospy.Time(0))
           print("trans:")
           print(trans)
           print("rot:")
           print(rot)
           object_position_pose(trans,rot)
       except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
           continue
       rate.sleep()

然后是控制机械臂的运动

2. 控制机械臂运动

#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import geometry_msgs
import tf

from moveit_commander import MoveGroupCommander

from moveit_python import (MoveGroupInterface,
                           PlanningSceneInterface,
                           PickPlaceInterface)
import moveit_msgs.msg
from geometry_msgs.msg import Pose
from copy import deepcopy

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()
#定义基于moveit的规划组，这边的"manipulator"是机械臂的规划祖，"gripper"是夹爪的规划组，如果你没有夹爪，就给注释掉。
    arm_group = moveit_commander.move_group.MoveGroupCommander("manipulator")
    hand_group = moveit_commander.move_group.MoveGroupCommander("gripper") 
   
    arm_group.set_named_target("home_j")#这边你在设置urdf模型时定义一些固定的位姿。这可以注释调，不用执行这一行，至于为什么，慢慢会明白。
    plan = arm_group.go()#控制机械臂移动到"home_j"位置。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
    #姿态的话，暂时用当前采集到的姿态。因为我用的是kinect采集的姿态，有点不稳。
    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



    arm_group.set_pose_target(pose_target)
    arm_group.go(wait=True)
    print("Point 3")#控制机械臂移动到目标上方。

    # Block point1
    pose_target.position.z = pose_target.position.z-0.07#向下移动0.07米
    arm_group.set_pose_target(pose_target)
    arm_group.go(wait=True)
    print("Point 4")
    rospy.sleep(3)

    hand_group.set_named_target("close")#关闭夹爪
    plan = hand_group.go()
    print("Point 5")
    rospy.sleep(2)

    pose_target.position.z = pose_target.position.z+0.25#夹起目标向上移动0.25米
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point 6")


    pose_target.position.x = pose_target.position.x + 0.5#向x轴方向移动0.5米
    arm_group.go()
    rospy.sleep(3)
    print("Point 7")



    hand_group.set_named_target("open")#打开夹爪。
    plan = hand_group.go()
    print("Point 8")
        
        
    moveit_commander.roscpp_shutdown()#任务完成，关闭moveit

def object_position_sub():
#订阅上面的发送转换关系的  话题
    rospy.Subscriber("/objection_position_pose",Pose,callback,queue_size=10)
if __name__ == "__main__":
    rospy.init_node('object_position_sub_And_grasp_node',anonymous=True)#初始化一个结点
    object_position_sub()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            print("11")
            rospy.spin()
        except KeyboardInterrupt:
            print("Shutting down")
        rate.sleep()

需要提出的是，通过这种方法，机械臂规划的路径是很奇葩的，即使是两个很近的点，机械臂都要绕来绕去，绕一大圈。这很麻烦。
怎么解决呢？
方法是有的，那就是在笛卡尔空间规划机械臂的运动。
具体方法参考这个链接。

3. 在笛卡尔空间规划末端路径

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy
 
class MoveItCartesianDemo:
    def __init__(self):
 
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
 
        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
 
        # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线
        cartesian = rospy.get_param('~cartesian', True)
                      
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')
        
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.001)
        
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
 
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
                                               
        # 获取当前位姿数据最为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # 初始化路点列表
        waypoints = []
 
        # 如果为True,将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)
            
        # 设置路点数据，并加入路点列表，所有的点都加入
        wpose = deepcopy(start_pose)#拷贝对象
        wpose.position.z -= 0.2
 
        if cartesian:  #如果设置为True，那么走直线
            waypoints.append(deepcopy(wpose))
        else:          #否则就走曲线
            arm.set_pose_target(wpose)  #自由曲线
            arm.go()
            rospy.sleep(1)
 
        wpose.position.x += 0.15
 
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
        
        wpose.position.y += 0.1
 
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
 
        wpose.position.x -= 0.15
        wpose.position.y -= 0.1
 
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
 
 
        #规划过程
 
        if cartesian:
		fraction = 0.0   #路径规划覆盖率
		maxtries = 100   #最大尝试规划次数
		attempts = 0     #已经尝试规划次数
		
		# 设置机器臂当前的状态作为运动初始状态
		arm.set_start_state_to_current_state()
	 
		# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
		while fraction < 1.0 and attempts < maxtries:
        #规划路径 ，fraction返回1代表规划成功
		    (plan, fraction) = arm.compute_cartesian_path (
		                            waypoints,   # waypoint poses，路点列表，这里是5个点
		                            0.01,        # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达
		                            0.0,         # jump_threshold，跳跃阈值，设置为0代表不允许跳跃
		                            True)        # avoid_collisions，避障规划
		    
		    # 尝试次数累加
		    attempts += 1
		    
		    # 打印运动规划进程
		    if attempts % 10 == 0:
		        rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
		             
		# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
		if fraction == 1.0:
		    rospy.loginfo("Path computed successfully. Moving the arm.")
		    arm.execute(plan)
		    rospy.loginfo("Path execution complete.")
		# 如果路径规划失败，则打印失败信息
		else:
		    rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  
 
		rospy.sleep(1)
 
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
 
if __name__ == "__main__":
    try:
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        pass

到这里，机械臂的运动已经可以控制的很清晰了。
但是，有时候，我们希望控制的末端在6个自由度的其中一个自由度的运动。
这个时候我们需要这么做。

4. 单独控制某一个自由度的运动

首先看代码，主要参考这个网页

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class MoveItIkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
                
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
                        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
                
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
               
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.2593
        target_pose.pose.position.y = 0.0636
        target_pose.pose.position.z = 0.1787
        target_pose.pose.orientation.x = 0.70692
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 0.70729
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)
         
        # 控制机械臂终端向右移动5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制机械臂终端反向旋转90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    MoveItIkDemo()

然后是代码解析。这边一定要看，

重要API整理

第一步初始化：与前面正运动初始化一样。

end_effector_link = arm.get_end_effector_link()

第二步：获取机械臂的终端link

reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)

第三步设置参考系：逆运动的位姿需要在笛卡尔坐标下描述，因此将base_link设置为指定的参考坐标系。

arm.set_named_target('home')
arm.go()
rospy.sleep(2)
arm.set_start_state_to_current_state()

第四步设置起始位姿：先将机械臂回到home位姿，然后设置该位姿为运动的起始位姿。

target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()     
target_pose.pose.position.x = 0.2593
target_pose.pose.position.y = 0.0636
target_pose.pose.position.z = 0.1787
target_pose.pose.orientation.x = 0.70692
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.70729
arm.set_pose_target(target_pose, end_effector_link)

第五步设置运动终点位姿：x,y,z描述end_effector_link在base_link坐标系下的空间位置；x,y,z,w四元数描述end_effector_link在base_link坐标系下的空间姿态。

traj = arm.plan()
arm.execute(traj)

第六步规划与执行：arm.plan（）规划一条起始位姿到终点位姿的路径traj, arm.execute(traj)执行该路径。

arm.shift_pose_target(1, -0.05, end_effector_link)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
rospy.sleep(1)

除了使用PoseStamped描述位姿并规划外，还可以使用shift_pose_target实现单轴方向上的目标设置与规划；
第一个参数：描述机器人在六个自由度中实现哪一种运动，0，1，2，3，4，5分别表示xyz三个方向的平移与旋转。
第二个参数：描述机器人移动或旋转的量，单位为m或者弧度。
第三个参数：描述该运动针对的对象。