坐标转换有问题

[2861496988]

是在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 = ''