是在Ubuntu18中进行的,我用的是gazebo中带的相机,并不是深度相机,因此这个深度我是自己定义的,在仿真中,定义的物体的位置为(-0.6,0.1,0.3165,0,0,0)输出的为 ('anode pose is: ', [-0.50000007943038327, -0.17674752279857978, 1.059866627149683])
并且机械臂的末端会朝上,我想要的是末端朝下,朝着物体
相机的位置为<origin xyz="-0.9 -0.0 1.0" rpy="0 ${M_PI/2} 0"/>
请问哪里出了问题呢?
以下是代码
#coding=utf-8
import cv2
import rospy
import tf
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
import numpy as np
import sys
import moveit_commander
from geometry_msgs.msg import PoseStamped
from std_srvs.srv import Empty
from yolov8_ros_msgs.msg import BoundingBox, BoundingBoxes
import random
class graspDemo:
def __init__(self):
# 机械臂规划参考系
self.reference_frame = 'world'
# 初始化moveit控制器
moveit_commander.roscpp_initializer.roscpp_initialize(sys.argv)
self.robot = moveit_commander.robot.RobotCommander()
# 初始化manipulator group
self.arm = moveit_commander.move_group.MoveGroupCommander("manipulator_e5")
self.end_effector_link = self.arm.get_end_effector_link()
# 设置容忍误差
self.arm.set_goal_position_tolerance(0.001)
self.arm.set_goal_orientation_tolerance(0.05)
self.arm.allow_replanning(True)
self.arm.set_pose_reference_frame(self.reference_frame)
def move(self, pose, orientation):
target_pose = PoseStamped()
# 设置消息头部的frame_id,即这个姿态数据参考的坐标帧
target_pose.header.frame_id = self.reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = pose[0]
target_pose.pose.position.y = pose[1]
target_pose.pose.position.z = pose[2]
target_pose.pose.orientation.x = orientation[0]
target_pose.pose.orientation.y = orientation[1]
target_pose.pose.orientation.z = orientation[2]
target_pose.pose.orientation.w = orientation[3]
# 设置当前坐标为起始坐标
self.arm.set_start_state_to_current_state()
# 设置位置目标
self.arm.set_pose_target(target_pose, self.end_effector_link)
self.arm.go(wait=True)
# 回到home姿态
def go_named(self, place = 'home'):
# 移动至命名的pose,允许用户指定一个预定义的姿态或位置目标
self.arm.set_named_target(place)
self.arm.go(wait=True)
@property
# 返回当前坐标
def poseNow(self):
return self.arm.get_current_pose(self.end_effector_link)
# 当前位置的基础上进行相对移动
def move_by_posNow(self, dx, dy, dz):
# 在当前坐标的基础上设置目标
self.arm.set_start_state_to_current_state()
pos = self.poseNow.pose
pos.position.x -= dx
pos.position.y -= dy
pos.position.z -= dz
self.arm.set_pose_target(pos, self.end_effector_link)
self.arm.go(wait=True)
# 指定的关节角度来控制机械臂的移动
def move_by_joints(self,joints):
# 设置关节坐标
self.arm.set_joint_value_target(joints)
state = self.arm.go(wait=True)
return state
# 用于控制机械臂通过笛卡尔路径规划(即直线移动)到达指定的三维空间位置 (x, y, z)
def move_cartesian(self, x, y, z):
# 笛卡尔规划
waypoints = []
pos = self.poseNow.pose
pos.position.x = x
pos.position.y = y
pos.position.z = z
waypoints.append(pos)
fraction = 0.0
maxtries = 100
attempts = 0
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_start_state_to_current_state()
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path (
waypoints, # waypoint poses,路点列表
0.1, # eef_step,终端步进值
0.0, # jump_threshold,跳跃阈值
True) # avoid_collisions,避障规划
attempts += 1
if fraction == 1.0:
self.arm.execute(plan)
return True
else:
pass
return False
def move_cartesian_byNow(self, x, y, z):
# 笛卡尔规划(从当前坐标设置目标)
waypoints = []
pos = self.poseNow.pose
# waypoints.append(pos)
pos.position.x -= x
pos.position.y -= y
pos.position.z -= z
# pos.orientation.x = preState['down'][0]
# pos.orientation.y = preState['down'][1]
# pos.orientation.z = preState['down'][2]
# pos.orientation.w = preState['down'][3]
waypoints.append(pos)
fraction = 0.0
maxtries = 100
attempts = 0
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_start_state_to_current_state()
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path (
waypoints, # waypoint poses,路点列表
0.1, # eef_step,终端步进值
0.0, # jump_threshold,跳跃阈值
True) # avoid_collisions,避障规划
attempts += 1
if fraction == 1.0:
self.arm.execute(plan)
return True
else:
pass
return False
def shutDowm(self):
# moveit关闭
moveit_commander.roscpp_initializer.roscpp_shutdown()
rospy.loginfo('grasp complete!!!')
def get_rotation_matrix(q):
# in TF, it is xyzw
# xyzw方向转旋转矩阵
x = q[0]
y = q[1]
z = q[2]
w = q[3]
rot = [[1-2*y*y-2*z*z, 2*x*y+2*w*z, 2*x*z-2*w*y],
[2*x*y-2*w*z, 1-2*x*x-2*z*z, 2*y*z+2*w*x],
[2*x*z+2*w*y, 2*y*z-2*w*x, 1-2*x*x-2*y*y]]
return rot
def get_CameraFrame_Pos(u, v, depthvalue):
# 将像素平面的点(pixel_x, pixel_y)转换到相机坐标(u、v图像坐标,depthValue对应坐标的深度值)
# fx fy cx cy为相机内参
fx = 420.93942150909646
fy = 420.93942150909646
cx = 320
cy = 240
z = float(depthvalue)
cam_x = float((u - cx) * z) / fx
cam_y = float((v - cy) * z) / fy
cam_z = z
return [cam_x, cam_y, cam_z, 1]
def get_RT_matrix(base_frame, reference_frame):
# 获取base_frame到reference_frame旋转平移矩阵,通过tf变换获取
listener = tf.TransformListener()
i = 3 # 尝试3次,三次未获取到则获取失败
while i!=0:
try:
listener.waitForTransform(base_frame, reference_frame,rospy.Time.now(), rospy.Duration(3.0))
camera2World = listener.lookupTransform(base_frame, reference_frame, rospy.Time(0))
break
except:
pass
i = i - 1
T = camera2World[0]
R = get_rotation_matrix(camera2World[1])
# 将其转换为一个4x4的齐次变换矩阵
R[0].append(0)
R[1].append(0)
R[2].append(0)
R.append([0.0,0.0,0.0,1.0])
R = np.mat(R)
return [R,T]
def coordinate_transform(cameraFrame_pos, R, T):
# 相机系转世界坐标系,先旋转再平移,转置操作,np.mat 函数创建一个矩阵
worldFrame_pos = R.I * np.mat(cameraFrame_pos).T
worldFrame_pos[0,0] = worldFrame_pos[0,0] + T[0]
worldFrame_pos[1,0] = worldFrame_pos[1,0] + T[1]
worldFrame_pos[2,0] = worldFrame_pos[2,0] + T[2]
worldFrame_pos = [worldFrame_pos[0,0], worldFrame_pos[1,0], worldFrame_pos[2,0]]
return worldFrame_pos
findObject = False
image = np.array(0)
imagehOK = False
def BoundingBoxCallBack(data):
# yolo检测的回调函数
global findObject, u, v, graspObject
if not findObject:
# 待抓取目标为空,则请求输入抓取目标
if graspObject == '':
graspObject = raw_input('object detected, please input the object you want to grasp:(anode, cathode)\n')
objectGrasp = []
for dat in data.bounding_boxes:
# 遍历所有目标,种类与待抓取目标相同则保存目标中心位置
if graspObject == dat.Class:
objectGrasp.append([dat.Class, (dat.xmin + dat.xmax)/2, (dat.ymin + dat.ymax)/2])
if objectGrasp != []:
# 如果待抓取目标存在,则在目标列表随机选择一个返回
rospy.loginfo('{} found, begin grasp!!!'.format(graspObject))
_, u, v = random.choice(objectGrasp)
findObject = True
else:
rospy.loginfo('The object you want to grasp is absent!!!')
def imageCallback(data):
# 深度图像回调函数
global image,imageOK
imageOK = False
image = CvBridge().imgmsg_to_cv2(data, data.encoding)
imageOK = True
# 末端向下对应的xyzw坐标
endDown = [0.0, 0.0, 0.0, 0.0]
def grasp_test(grasp_demo):
if __name__ == "__main__":
# 图像和检测框对应topic名称
image_topic = '/camera/image_raw'
BoundingBox_topic = '/yolov8/BoundingBoxes'
# 初始化ros节点
rospy.init_node('grasp')
# 实例化抓取控制类
grasp_demo = graspDemo()
# 机械臂移动到该位置
grasp_demo.go_named('home')
# 初始化图像系坐标及待抓取物体
u = 320
v = 240
graspObject = ''
# 订阅图像和检测框对应topic,BoundingBoxes是消息类型,定义了消息的数据结构
rospy.Subscriber(BoundingBox_topic, BoundingBoxes, BoundingBoxCallBack, queue_size=1)
rospy.Subscriber(image_topic, Image, imageCallback, queue_size=1)
while not rospy.is_shutdown():
# 如果检测到待抓取物体且获取到了图
if findObject :
# 获取相机系到世界系的旋转平移矩阵
[R, T] = get_RT_matrix('world','camera_link')
print("[R, T]",[R, T])
u, v = int(u), int(v) # 防止检测的坐标越界
print("u,v",u,v)
# 像素坐标转相机坐标
cameraFrame_pos = get_CameraFrame_Pos(u, v, 0.4)
print(cameraFrame_pos)
# 相机坐标转世界坐标
worldFrame_pos = coordinate_transform(cameraFrame_pos, R, T)
print(graspObject + ' pose is: ', worldFrame_pos)
# 移动到抓取物体的上方
grasp_demo.move([worldFrame_pos[0], worldFrame_pos[1], worldFrame_pos[2]],endDown)
rospy.sleep(0.5) # wait for grasp
# 向上移动仿真防止直接移动吸到其他物体
grasp_demo.move_cartesian_byNow(0, 0 , -0.1) # avoid grasp other object by mistake
# 移动到放置位置
#grasp_demo.move([worldFrame_pos[0]+0.4, worldFrame_pos[1]-0.2, worldFrame_pos[2]-1.0],endDown)
rospy.sleep(0.5)
# 回到检测的位置
grasp_demo.go_named('home')
# 重置参数
findObject = False
graspObject = ''