键盘打字
案例视频
案例代码
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
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))
left_x_dis = size * 1.3
left_y_dis = size * 1.35
right_x_dis = size * 1.3
right_y_dis = size * 1.35
rad = int(size / 2)
left_add_x = [size * 1.25]
left_add_y = [-size * 2, -size * 2.3, -size * 3]
right_add_x = [size * 1.3]
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":
add_x = left_add_x
add_y = left_add_y
keyboard_txt = left_keyboard_txt
x_dis = left_x_dis
y_dis = left_y_dis
else:
add_x = right_add_x
add_y = right_add_y
keyboard_txt = right_keyboard_txt
x_dis = right_x_dis
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
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
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
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()
← 上一页 | 下一章 →