话不多说了

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

首先是安装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 pythonimport rospy
import tf
from geometry_msgs.msg import Pose
def object_position_pose(t,o):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():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):continuerate.sleep()

然后是控制机械臂的运动

2. 控制机械臂运动

#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import geometry_msgs
import tffrom moveit_commander import MoveGroupCommanderfrom moveit_python import (MoveGroupInterface,PlanningSceneInterface,PickPlaceInterface)
import moveit_msgs.msg
from geometry_msgs.msg import Pose
from copy import deepcopydef callback(pose):object_position_info = pose.positionobject_orientation_info = pose.orientationprint object_position_infomoveit_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 toppose_target.position.x = object_position_info.xpose_target.position.y = object_position_info.ypose_target.position.z = object_position_info.z+0.25arm_group.set_pose_target(pose_target)arm_group.go(wait=True)print("Point 3")# Block point1pose_target.position.z = pose_target.position.z-0.07arm_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.25arm_group.set_pose_target(pose_target)plan = arm_group.go()print("Point 6")pose_target.position.x = pose_target.position.x + 0.5arm_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()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 deepcopyclass MoveItCartesianDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_cartesian_demo', anonymous=True)# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线cartesian = rospy.get_param('~cartesian', True)# 初始化需要使用move group控制的机械臂中的arm grouparm = 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.2if cartesian:  #如果设置为True，那么走直线waypoints.append(deepcopy(wpose))else:          #否则就走曲线arm.set_pose_target(wpose)  #自由曲线arm.go()rospy.sleep(1)wpose.position.x += 0.15if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.y += 0.1if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.x -= 0.15wpose.position.y -= 0.1if 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)# 关闭并退出moveitmoveit_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 JointTrajectoryPointfrom geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_eulerclass MoveItIkDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_ik_demo')# 初始化需要使用move group控制的机械臂中的arm grouparm = 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_frametarget_pose.header.stamp = rospy.Time.now()     target_pose.pose.position.x = 0.2593target_pose.pose.position.y = 0.0636target_pose.pose.position.z = 0.1787target_pose.pose.orientation.x = 0.70692target_pose.pose.orientation.y = 0.0target_pose.pose.orientation.z = 0.0target_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)# 控制机械臂终端向右移动5cmarm.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()# 关闭并退出moveitmoveit_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或者弧度。
第三个参数：描述该运动针对的对象。