task_3.py 文件源码

def scoopSpoon(self):
        while not (self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/j2n6a300_link_finger_tip_3")):
            pass

        print ("Publishing transform of figner wrt endEffector frame")
        p.listen.waitForTransform('/j2n6a300_end_effector','/j2n6a300_link_finger_tip_3',rospy.Time(),rospy.Duration(100.0))
        t = self.listen.getLatestCommonTime("/j2n6a300_end_effector", "/j2n6a300_link_finger_tip_3")
        translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector", "/j2n6a300_link_finger_tip_3", t)
        # print (translation)
        matrix2 = self.listen.fromTranslationRotation(translation, quaternion)
        # print (matrix2)
        # required_cartvelo = [0,0,0,0.1,0,0]
        quaternion = pose_action_client.EulerXYZ2Quaternion([0.15,0,0]) # set rot angle HERE (deg)
        matrix1 = self.listen.fromTranslationRotation([0,0,0], quaternion)
        # print (matrix1)
        matrix3 = np.dot(matrix2,matrix1)
        scale, shear, rpy_angles, trans, perps = tf.transformations.decompose_matrix(matrix3)
        trans = list(trans.tolist())
        rpy_angles = list(rpy_angles)
        # euler = pose_action_client.Quaternion2EulerXYZ(quat_1)
        # pose = trans + rpy_angles
        return pose
        # return translation, quaternion