键盘打字

案例视频

案例代码

import json
import threading

from uvc_camera import UVCCamera
import cv2
import time
import numpy as np
from transformation import *
from pymycobot import Mercury
import typing as T
import cv2.aruco as aruco
from arm_control import *
from marker_utils import *
import stag
from matplotlib.path import Path


# 将旋转矩阵转为欧拉角
def CvtRotationMatrixToEulerAngle(pdtRotationMatrix):
    pdtEulerAngle = np.zeros(3)
    pdtEulerAngle[2] = np.arctan2(pdtRotationMatrix[1, 0], pdtRotationMatrix[0, 0])
    fCosRoll = np.cos(pdtEulerAngle[2])
    fSinRoll = np.sin(pdtEulerAngle[2])
    pdtEulerAngle[1] = np.arctan2(-pdtRotationMatrix[2, 0],
                                  (fCosRoll * pdtRotationMatrix[0, 0]) + (fSinRoll * pdtRotationMatrix[1, 0]))
    pdtEulerAngle[0] = np.arctan2((fSinRoll * pdtRotationMatrix[0, 2]) - (fCosRoll * pdtRotationMatrix[1, 2]),
                                  (-fSinRoll * pdtRotationMatrix[0, 1]) + (fCosRoll * pdtRotationMatrix[1, 1]))
    return pdtEulerAngle


# 获取物体坐标(相机系)
def calc_markers_base_position(corners: NDArray, ids: T.List, marker_size: int, mtx: NDArray, dist: NDArray) -> T.List:
    if len(corners) == 0:
        return []
    rvecs, tvecs = solve_marker_pnp(corners, marker_size, mtx, dist)
    res = []
    for i, tvec, rvec in zip(ids, tvecs, rvecs):
        tvec = tvec.squeeze().tolist()
        rvec = rvec.squeeze().tolist()
        rotvector = np.array([[rvec[0], rvec[1], rvec[2]]])
        Rotation = cv2.Rodrigues(rotvector)[0]
        Euler = CvtRotationMatrixToEulerAngle(Rotation)
        cam_coords = tvec + rvec
        target_coords = cam_coords
    return target_coords


# 将欧拉角转为旋转矩阵
def CvtEulerAngleToRotationMatrix(ptrEulerAngle):
    ptrSinAngle = np.sin(ptrEulerAngle)
    ptrCosAngle = np.cos(ptrEulerAngle)
    ptrRotationMatrix = np.zeros((3, 3))
    ptrRotationMatrix[0, 0] = ptrCosAngle[2] * ptrCosAngle[1]
    ptrRotationMatrix[0, 1] = ptrCosAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] - ptrSinAngle[2] * ptrCosAngle[0]
    ptrRotationMatrix[0, 2] = ptrCosAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] + ptrSinAngle[2] * ptrSinAngle[0]
    ptrRotationMatrix[1, 0] = ptrSinAngle[2] * ptrCosAngle[1]
    ptrRotationMatrix[1, 1] = ptrSinAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] + ptrCosAngle[2] * ptrCosAngle[0]
    ptrRotationMatrix[1, 2] = ptrSinAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] - ptrCosAngle[2] * ptrSinAngle[0]
    ptrRotationMatrix[2, 0] = -ptrSinAngle[1]
    ptrRotationMatrix[2, 1] = ptrCosAngle[1] * ptrSinAngle[0]
    ptrRotationMatrix[2, 2] = ptrCosAngle[1] * ptrCosAngle[0]
    return ptrRotationMatrix


# 坐标转换为齐次变换矩阵，（x,y,z,rx,ry,rz）单位rad
def Transformation_matrix(coord):
    position_robot = coord[:3]
    pose_robot = coord[3:]
    RBT = CvtEulerAngleToRotationMatrix(pose_robot)
    PBT = np.array([[position_robot[0]],
                    [position_robot[1]],
                    [position_robot[2]]])
    temp = np.concatenate((RBT, PBT), axis=1)
    array_1x4 = np.array([[0, 0, 0, 1]])
    # 将两个数组按行拼接起来
    matrix = np.concatenate((temp, array_1x4), axis=0)
    return matrix


def Eyes_in_hand(coord, camera, arm):
    # 相机坐标
    Position_Camera = np.transpose(camera[:3])
    # 机械臂坐标矩阵
    Matrix_BT = Transformation_matrix(coord)
    # 手眼矩阵
    if arm == "left":
        Matrix_TC = np.array([[0, -1, 0, Camera_LEN],
                              [1, 0, 0, -3],
                              [0, 0, 1, -Tool_LEN],
                              [0, 0, 0, 1]])
    else:
        Matrix_TC = np.array([[0, 1, 0, Camera_LEN],
                              [-1, 0, 0, 28],
                              [0, 0, 1, -Tool_LEN],
                              [0, 0, 0, 1]])
    # 物体坐标（相机系）
    Position_Camera = np.append(Position_Camera, 1)
    Position_B = Matrix_BT @ Matrix_TC @ Position_Camera
    return Position_B


# 等待双臂运行结束
def wait():
    time.sleep(0.2)
    while (ml.is_moving() or mr.is_moving()):
        time.sleep(0.03)


# 等待左臂运行结束
def waitL():
    time.sleep(0.2)
    while (ml.is_moving()):
        time.sleep(0.03)


# 等待右臂运行结束
def waitR():
    time.sleep(0.2)
    while (mr.is_moving()):
        time.sleep(0.03)


# 画出键盘框，读取各字母坐标
def draw(frame, arm):
    global per_right_corners, per_left_corners
    # 将图片灰度
    imGray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # 获取图片中二维码的角点
    (corners, ids, rejected_corners) = stag.detectMarkers(imGray, 11)
    # 通过角点获取，二维码相对于相机的位移、旋转向量
    marker_pos_pack = calc_markers_base_position(corners, ids, marker_size, mtx, dist)
    if arm == "left":
        # 获取当前机械臂末端坐标
        stag_cur_coords = np.array(ml.get_base_coords())
        stag_cur_bcl = stag_cur_coords.copy()
        stag_cur_bcl[-3:] *= (np.pi / 180)
        # 通过手眼矩阵获取二维码相对于机械臂基座的坐标
        stag_fact_bcl = Eyes_in_hand(stag_cur_bcl, marker_pos_pack, "left")
        stag_coord = stag_cur_coords.copy()
        stag_coord[0] = stag_fact_bcl[0]
        stag_coord[1] = stag_fact_bcl[1]
        stag_coord[2] = stag_fact_bcl[2]
        # 存入二维码的三维坐标
        keyboard_coords[","] = stag_coord
    else:
        # 获取当前机械臂末端坐标
        stag_cur_coords = np.array(mr.get_base_coords())
        stag_cur_bcl = stag_cur_coords.copy()
        stag_cur_bcl[-3:] *= (np.pi / 180)
        # 通过手眼矩阵获取二维码相对于机械臂基座的坐标
        stag_fact_bcl = Eyes_in_hand(stag_cur_bcl, marker_pos_pack, "right")
        stag_coord = stag_cur_coords.copy()
        stag_coord[0] = stag_fact_bcl[0]
        stag_coord[1] = stag_fact_bcl[1]
        stag_coord[2] = stag_fact_bcl[2]
        # 存入二维码的三维坐标
        keyboard_coords["."] = stag_coord
    # 通过角点获取，二维码相对于相机的位移、旋转向量
    rvecs, tvecs = solve_marker_pnp(corners, marker_size, mtx, dist)
    # 画出坐标系
    cv2.drawFrameAxes(frame, mtx, dist, rvecs, tvecs, 50)
    draw_img = frame

    x1 = corners[0][0][0][0]
    y1 = corners[0][0][0][1]
    x2 = corners[0][0][1][0]
    y2 = corners[0][0][1][1]

    x = x1 - x2
    y = y1 - y2
    # 根据两个交点之间连线的角度获取偏转角
    r = np.arctan(y / x)
    r = abs(r)
    # 获取两个角点之间的距离
    size = abs(x / np.cos(r))
    # 左臂摄像头两个按键x轴之间的距离
    left_x_dis = size * 1.3
    # 左臂摄像头两个按键y轴之间的距离
    left_y_dis = size * 1.35
    # 右臂摄像头两个按键x轴之间的距离
    right_x_dis = size * 1.3
    # 右臂摄像头两个按键y轴之间的距离
    right_y_dis = size * 1.35
    # 按键框的半径
    rad = int(size / 2)
    # 左臂摄像头x轴与第一个字母的偏移距离
    left_add_x = [size * 1.25]
    # 左臂摄像头y轴各行与第一个字母的偏移距离
    left_add_y = [-size * 2, -size * 2.3, -size * 3]
    # 右臂摄像头x轴与第一个字母的偏移距离
    right_add_x = [size * 1.3]
    # 右臂摄像头y轴各行与第一个字母的偏移距离
    right_add_y = [size * 4.1, size * 2.1, size * 1]
    # 获取按键框的中心点
    tray_frame = Path(corners[0][0])
    tray_frame_center_plot = tray_frame.vertices.mean(axis=0)

    # 画出按键框
    def draw_keyboard(arm):
        if arm == "left":
            # 摄像头x轴与第一个字母的偏移距离
            add_x = left_add_x
            # 左臂摄像头y轴各行与第一个字母的偏移距离
            add_y = left_add_y
            # 左臂按键布局
            keyboard_txt = left_keyboard_txt
            # 左臂摄像头两个按键x轴之间的距离
            x_dis = left_x_dis
            # 左臂摄像头两个按键y轴之间的距离
            y_dis = left_y_dis
        else:
            # 摄像头x轴与第一个字母的偏移距离
            add_x = right_add_x
            # 摄像头y轴各行与第一个字母的偏移距离
            add_y = right_add_y
            # 右臂按键布局
            keyboard_txt = right_keyboard_txt
            # 右臂摄像头两个按键x轴之间的距离
            x_dis = right_x_dis
            # 右臂摄像头两个按键y轴之间的距离
            y_dis = right_y_dis

        # 循环绘制每行每列的按键框
        for j in range(len(keyboard_txt)):
            for i in range(len(keyboard_txt[j])):
                # 获取对应的按键字母
                txt = keyboard_txt[j][i]
                # 获取对应按键的二维坐标
                if arm == "left":
                    x_key = int(tray_frame_center_plot[0] + add_x[0] + x_dis * j)
                    y_key = int(tray_frame_center_plot[1] + add_y[j] - y_dis * i)
                else:
                    x_key = int(tray_frame_center_plot[0] - x_dis * j)
                    y_key = int(tray_frame_center_plot[1] + add_y[j] + y_dis * i)
                if y1 < y2:
                    rr = -r
                else:
                    rr = r
                row = draw_img.shape[0]

                x_key = x_key
                y_key = row - y_key
                x_center = tray_frame_center_plot[0]
                y_center = row - tray_frame_center_plot[1]
                # 获取图像中一个点绕另一个点旋转后的位置
                x_end = (x_key - x_center) * np.cos(rr) - (y_key - y_center) * np.sin(rr) + x_center
                y_end = (x_key - x_center) * np.sin(rr) + (y_key - y_center) * np.cos(rr) + y_center
                x_end = int(x_end)
                y_end = int(row - y_end)
                # 计算出这个按键的角点
                key_corners = (np.array(
                    [[(x_end - size / 2, y_end - size / 2),
                      (x_end + size / 2, y_end - size / 2),
                      (x_end + size / 2, y_end + size / 2),
                      (x_end - size / 2, y_end + size / 2)]],
                    dtype=np.float32,
                ),)
                # 获取按键的坐标(相机系)
                marker_pos_pack = calc_markers_base_position(key_corners, ids, marker_size, mtx, dist)
                if arm == "left":
                    # 获取机械臂当前坐标
                    left_cur_coords = np.array(ml.get_base_coords())
                    left_cur_bcl = left_cur_coords.copy()
                    left_cur_bcl[-3:] *= (np.pi / 180)
                    # 通过矩阵变化将按键坐标(相机系)转成(基坐标系)
                    left_fact_bcl = Eyes_in_hand(left_cur_bcl, marker_pos_pack, "left")
                    left_end_coord = left_cur_coords.copy()
                    left_end_coord[0] = left_fact_bcl[0]
                    left_end_coord[1] = left_fact_bcl[1]
                    left_end_coord[2] = stag_coord[2]
                    # 将按键位置保存入字典中，方便后续点按
                    if txt in left_control:
                        keyboard_coords[txt] = left_end_coord
                else:
                    # 获取机械臂当前坐标
                    right_cur_coords = np.array(mr.get_base_coords())
                    right_cur_bcl = right_cur_coords.copy()
                    right_cur_bcl[-3:] *= (np.pi / 180)
                    # 通过矩阵变化将按键坐标(相机系)转成(基坐标系)
                    right_fact_bcl = Eyes_in_hand(right_cur_bcl, marker_pos_pack, "right")
                    right_end_coord = right_cur_coords.copy()
                    right_end_coord[0] = right_fact_bcl[0]
                    right_end_coord[1] = right_fact_bcl[1]
                    right_end_coord[2] = stag_coord[2]
                    # 将按键位置保存入字典中，方便后续点按
                    if txt in right_control:
                        keyboard_coords[txt] = right_end_coord
                # 画出按键的框
                cv2.circle(draw_img, (x_end, y_end), rad, (0, 0, 255), 2)

    draw_keyboard(arm)
    return draw_img


# 左右壁微调量
l_x = 3
l_y = 3
r_x = - 5
r_y = -10


def keyboard():
    while True:
        key_code = cv2.waitKey(1) & 0xFF
        if key_code != 255:
            key_code = chr(key_code)
            if key_code in left_control:
                # 读取对应按键的三维坐标
                coords = keyboard_coords[key_code].copy()
                coords[0] = coords[0] + l_x
                coords[1] = coords[1] + l_y
                coords[2] = 315
                # 控制点按按键
                ml.send_base_coords(coords, sp)
                waitL()
                ml.send_base_coord(3, 305, sp)
                waitL()
                ml.send_base_coords(coords, sp)
                waitL()
                ml.send_angles(ml_pos, sp)
                waitL()
                print(coords)
            elif key_code in right_control:
                # 读取对应按键的三维坐标
                coords = keyboard_coords[key_code].copy()
                coords[0] = coords[0] + r_x
                coords[1] = coords[1] + r_y
                coords[2] = 330
                # 控制点按按键
                mr.send_base_coords(coords, sp)
                waitR()
                mr.send_base_coord(3, 320, sp)
                waitR()
                mr.send_base_coords(coords, sp)
                waitR()
                mr.send_angles(mr_pos, sp)
                waitR()
                print(coords)
            print("===============================================")


def write_string(txt):
    per_key = None
    # 循环点击按键
    for key_code in txt:
        cv2.waitKey(1)
        if key_code in left_control:
            # 读取上一个按键是哪个臂，如果是同个臂则连续点击，不是则另一个臂回初始点
            if per_key not in left_control:
                # 回到初始点
                mr.send_angles(mr_pos, sp)
                waitR()
            per_key = key_code
            # 读取对应按键的三维坐标
            coords = keyboard_coords[key_code].copy()
            coords[0] = coords[0] + l_x
            coords[1] = coords[1] + l_y
            coords[2] = 315
            # 控制机械臂点按
            ml.send_base_coords(coords, sp)
            waitL()
            ml.send_base_coord(3, 305, sp)
            waitL()
            ml.send_base_coords(coords, sp)
            waitL()
            print(coords)
        elif key_code in right_control:
            # 读取上一个按键是哪个臂，如果是同个臂则连续点击，不是则另一个臂回初始点
            if per_key not in right_control:
                # 回到初始点
                ml.send_angles(ml_pos, sp)
                waitL()
            per_key = key_code
            # 读取对应按键的三维坐标
            coords = keyboard_coords[key_code].copy()
            coords[0] = coords[0] + r_x
            coords[1] = coords[1] + r_y
            coords[2] = 330
            # 控制机械臂点按
            mr.send_base_coords(coords, sp)
            waitR()
            mr.send_base_coord(3, 320, sp)
            waitR()
            mr.send_base_coords(coords, sp)
            waitR()
            print(coords)
        print("===============================================")


# 打开相机读取画面
def camera():
    per_frame_left = None
    per_frame_right = None
    while True:
        left_camera.update_frame()
        left_frame = left_camera.color_frame()
        try:
            left_frame = draw(left_frame, "left")
            per_frame_left = left_frame
        except Exception as e:
            pass
        right_camera.update_frame()
        right_frame = right_camera.color_frame()
        try:
            right_frame = draw(right_frame, "right")
            per_frame_right = right_frame
        except Exception as e:
            pass
        # 旋转图像
        left_turn_img = cv2.rotate(left_frame, cv2.ROTATE_90_CLOCKWISE)
        right_turn_img = cv2.rotate(right_frame, cv2.ROTATE_90_CLOCKWISE)
        # 拼接图像
        imgs = np.hstack([left_turn_img, right_turn_img])
        # 展示图片
        cv2.imshow("calibed", imgs)
        key_code = cv2.waitKey(1) & 0xFF
        # 当按下q键时，截取图片进行操作
        if chr(key_code) == "q":
            left_turn_img = cv2.rotate(per_frame_left, cv2.ROTATE_90_CLOCKWISE)
            right_turn_img = cv2.rotate(per_frame_right, cv2.ROTATE_90_CLOCKWISE)
            imgs = np.hstack([left_turn_img, right_turn_img])
            cv2.imshow("calibed", imgs)
            break
    # 控制双臂点击键盘helloworld
    write_string("helloworld")


if __name__ == "__main__":
    # 设置左右壁端口
    mr = Mercury("/dev/ttyACM0")
    ml = Mercury("/dev/ttyTHS0")
    # 储存左右臂读取二维码的角点值
    per_left_corners = []
    per_right_corners = []
    sp = 60    # 机械臂运动速度
    Tool_LEN = 175  # 夹爪与与法兰的距离
    Camera_LEN = 78  # 相机与法兰的距离
    # 设置左右臂初始点角度
    ml_pos = [-31.92, -7.36, 0.0, -110.82, 81.75, 59.04, 44.25]
    mr_pos = [27.12, 28.34, 0.0, -108.77, -69.42, 71.92, -9.52]
    # 左臂键盘布局
    left_keyboard_txt = [
        ["q", "w", "e", "r", "t", "y", "u", "i", "o", "p"],
        ["a", "s", "d", "f", "g", "h", "j", "k", "l"],
        ["z", "x", "c", "v", "b", "n", "m"]
    ]
    # 右臂键盘布局
    right_keyboard_txt = [
        ["m", "n", "b", "v", "c", "x", "z"],
        ["l", "k", "j", "h", "g", "f", "d", "s", "a"],
        ["p", "o", "i", "u", "y", "t", "r", "e", "w", "q"],
    ]
    # 左臂点按的字母
    left_control = ["q", "w", "e", "r", "t",
                    "a", "s", "d", "f", "g",
                    "z", "x", "c", "v", ","]
    # 左臂点按的字母
    right_control = ["y", "u", "i", "o", "p",
                     "h", "j", "k", "l",
                     "b", "n", "m", "."]
    # 储存全部键盘按键的三维坐标
    keyboard_coords = {}

    np.set_printoptions(suppress=True, formatter={'float_kind': '{:.2f}'.format})
    # 相机配置文件
    camera_params = np.load("src/camera_params.npz")
    mtx, dist = camera_params["mtx"], camera_params["dist"]
    # 二维码大小
    marker_size = 15
    # 设置左右臂相机id
    left_camera = UVCCamera(3, mtx, dist)
    left_camera.capture()
    right_camera = UVCCamera(2, mtx, dist)
    right_camera.capture()
    # 左右壁移动到初始点
    mr.send_angles(mr_pos, sp)
    ml.send_angles(ml_pos, sp)
    # 等待运动结束
    wait()
    time.sleep(2)
    camera()
    # threading.Thread(target=camera).start()
    # while True:
    #     s = input("键盘字母：")
    #     if s in left_control:
    #         coords = keyboard_coords[s]
    #         print(coords)
    #
    #     elif s in right_control:
    #         coords = keyboard_coords[s]
    #         print(coords)

← 上一页 | 下一章 →