实际的机械臂控制（8）使用find_object3D和Kinect2实现目标跟踪(基于python)
作者：互联网
话不多说了
在很多大佬的博客，主要是古月居的一些博客中，他们都介绍了使用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 python
import 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):
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()
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
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
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
arm_group.set_pose_target(pose_target)
plan = arm_group.go()
print("Point 6")
pose_target.position.x = pose_target.position.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()
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或者弧度。
第三个参数：描述该运动针对的对象。
标签：set,target,python,pose,object3D,rospy,Kinect2,position,arm 来源： https://blog.csdn.net/qq_33328642/article/details/122667192