commit 25e83f05dbf4a30768fd41ce636184420f946e42 Author: raiot Date: Wed Aug 28 13:48:46 2024 +0800 init diff --git a/arm_debug.py b/arm_debug.py new file mode 100644 index 0000000..283efc0 --- /dev/null +++ b/arm_debug.py @@ -0,0 +1,29 @@ +import hiwonder +import time +# from utils.PixelToRealworld.predefinedconstants import * +from utils.pixel_convert import pixel2robot + +jetmax = hiwonder.JetMax() +sucker = hiwonder.Sucker() + +jetmax.go_home(1) +time.sleep(1) + +ort_point = (0, -162.94, 212.8 - 28) + +new_point = ort_point +new_point = (new_point[0] , new_point[1] - 80, new_point[2] - 130) + +ele_point = (-110, -50.94, 75) + +# px, py = input("请输入像素坐标：").split() +# px, py = int(px), int(py) +target = pixel2robot(877, 628) +print(target) + +# jetmax.set_position((target[0], target[1], -30), 1) +# jetmax.set_position((-75.41654829, -156.86319459, 70), 1) +jetmax.set_position(ele_point, 1) +time.sleep(1) + +print(jetmax.position) diff --git a/game_plat.py b/game_plat.py new file mode 100644 index 0000000..6ec8695 --- /dev/null +++ b/game_plat.py @@ -0,0 +1,430 @@ +import pygame +import math +import numpy as np +import random +from utils.cv_marker import cap_and_mark +from utils.stack_exe import Stackbot +import cv2 + + +pygame.init() + +WIDTH, HEIGHT = 800, 600 +screen = pygame.display.set_mode((WIDTH, HEIGHT)) +pygame.display.set_caption("积木塔仿真") + +WHITE = (255, 255, 255) +BLACK = (0, 0, 0) +RED = (255, 0, 0) + +LAYER_COLORS = [ + (255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (255, 0, 255), + (0, 255, 255), (128, 0, 0), (0, 128, 0), (0, 0, 128), (128, 128, 0), +] + +cap = cv2.VideoCapture(1) +cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G')) + +cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) +cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) + +class Block: + def __init__(self, x, y, width, height, angle, layer): + self.x = x + self.y = y + self.width = width + self.height = height + self.angle = angle + self.layer = layer + self.color = LAYER_COLORS[layer % len(LAYER_COLORS)] + + def draw(self, surface): + rotated_surface = pygame.Surface((self.width, self.height), pygame.SRCALPHA) + rotated_surface.set_alpha(125) + pygame.draw.rect(rotated_surface, self.color, (0, 0, self.width, self.height)) + rotated_surface = pygame.transform.rotate(rotated_surface, self.angle) + surface.blit(rotated_surface, (self.x - rotated_surface.get_width() // 2, self.y - rotated_surface.get_height() // 2)) + + def get_corners(self): + w, h = self.width / 2, self.height / 2 + corners = [(-w, -h), (w, -h), (w, h), (-w, h)] + angle_rad = np.deg2rad(self.angle) + rotation_matrix = np.array([ + [np.cos(angle_rad), -np.sin(angle_rad)], + [np.sin(angle_rad), np.cos(angle_rad)] + ]) + rotated_corners = np.dot(corners, rotation_matrix) + translated_corners = rotated_corners + np.array([self.x, self.y]) + # print(translated_corners) + rotated_corners = [(float(point[0]), float(point[1])) for point in translated_corners] + + return rotated_corners + + +class Tower: + def __init__(self): + self.blocks = [] + self.current_layer = 0 + self.layer_centroids = [] # 存储每层的重心 + self.stability_threshold = 10 # 稳定性阈值（像素） + self.rotation_angle = 15 # 预设每层旋转角度 + self.angle_tolerance = 5 # 允许的角度偏移 + self.position_tolerance = 10 # 允许的位置偏移 + + self.sim_to_robot_matrix = np.array([ + [1, 0, 0, -300], # 假设的转换矩阵，需要根据实际情况调整 + [0, -1, 0, -200], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]) + self.robot_to_sim_matrix = np.linalg.inv(self.sim_to_robot_matrix) + + self.stack_bot = Stackbot() + + self.is_upper = False + + + def sim_to_robot_coords(self, x, y, angle): + sim_coords = np.array([x, y, 0, 1]) + robot_coords = self.sim_to_robot_matrix.dot(sim_coords) + robot_angle = (angle + 360) % 360 # 确保角度在0-359范围内 + return robot_coords[0], robot_coords[1], robot_angle + + def robot_to_sim_coords(self, x, y, angle): + robot_coords = np.array([x, y, 0, 1]) + sim_coords = self.robot_to_sim_matrix.dot(robot_coords) + sim_angle = (angle + 360) % 360 + return sim_coords[0], sim_coords[1], sim_angle + + def add_block_from_robot(self, x, y, angle): + """ + 这个函数根据机器人的坐标和角度，在模拟世界中添加一个方块 + + 参数: + x (float): 机器人的 x 坐标 + y (float): 机器人的 y 坐标 + angle (float): 机器人的角度 + + 返回值： + bool：如果方块添加成功，返回 True，否则返回 False + + 注意：这个函数需要先调用 robot_to_sim_coords 来将机器人坐标转换为模拟坐标，并且需要一个名为 Block 的类来创建方块对象 + """ + sim_x, sim_y, sim_angle = self.robot_to_sim_coords(x, y, angle) + new_block = Block(sim_x, sim_y, 100, 20, sim_angle, self.current_layer) + if self.add_block(new_block): + print(f"Block added from robot at ({sim_x:.2f}, {sim_y:.2f}) with angle {sim_angle}") + return True + else: + print(f"Failed to add block from robot at ({sim_x:.2f}, {sim_y:.2f}) with angle {sim_angle}") + return False + + + def add_block(self, block): + if self.current_layer == 5: + self.is_upper = True + print("is upper") + + if not self.check_interference(block): + self.blocks.append(block) + self.stack_bot.pick_stack() + robo_xyz = self.sim_to_robot_coords(block.x, block.y, block.angle) + print(block.x, block.y, block.angle) + if self.is_upper: + self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer - 5) + else: + self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer) + return True + return False + + def draw(self, surface): + for block in self.blocks: + block.draw(surface) + + def check_stability(self): + current_layer_centroid = self.calculate_current_layer_centroid() + tower_centroid = self.calculate_tower_centroid() + + if current_layer_centroid is None or tower_centroid is None: + return True # 如果没有足够的块来计算重心，我们假设它是稳定的 + + horizontal_distance = abs(current_layer_centroid[0] - tower_centroid[0]) + + return horizontal_distance <= self.stability_threshold + + def blocks_overlap(self, block1, block2): + corners1 = block1.get_corners() + corners2 = block2.get_corners() + + # 检查 block1 的角是否在 block2 内部 + for corner in corners1: + if self.point_inside_polygon(corner, corners2): + return True + + # 检查 block2 的角是否在 block1 内部 + for corner in corners2: + if self.point_inside_polygon(corner, corners1): + return True + + # 检查边是否相交 + for i in range(4): + for j in range(4): + if self.line_intersects(corners1[i], corners1[(i+1)%4], corners2[j], corners2[(j+1)%4]): + return True + + return False + + def point_inside_polygon(self, point, polygon): + x, y = point + n = len(polygon) + inside = False + p1x, p1y = polygon[0] + for i in range(n + 1): + p2x, p2y = polygon[i % n] + if y > min(p1y, p2y): + if y <= max(p1y, p2y): + if x <= max(p1x, p2x): + if p1y != p2y: + xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x + if p1x == p2x or x <= xinters: + inside = not inside + p1x, p1y = p2x, p2y + return inside + + def line_intersects(self, p1, p2, p3, p4): + def ccw(A, B, C): + return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0]) + return ccw(p1,p3,p4) != ccw(p2,p3,p4) and ccw(p1,p2,p3) != ccw(p1,p2,p4) + + def check_interference(self, new_block): + # 检查是否与其他木块重叠 + for block in self.blocks: + if block.layer == new_block.layer and self.blocks_overlap(block, new_block): + return True + return False + + def calculate_current_layer_centroid(self): + current_layer_blocks = [block for block in self.blocks if block.layer == self.current_layer] + if not current_layer_blocks: + return None + + total_area = 0 + weighted_x = 0 + weighted_y = 0 + + for block in current_layer_blocks: + area = block.width * block.height + total_area += area + weighted_x += block.x * area + weighted_y += block.y * area + + if total_area == 0: + return None + + centroid_x = weighted_x / total_area + centroid_y = weighted_y / total_area + return (centroid_x, centroid_y) + + def calculate_tower_centroid(self): + if not self.blocks: + return None + + total_area = 0 + weighted_x = 0 + weighted_y = 0 + + for block in self.blocks: + area = block.width * block.height + total_area += area + weighted_x += block.x * area + weighted_y += block.y * area + + if total_area == 0: + return None + + centroid_x = weighted_x / total_area + centroid_y = weighted_y / total_area + return (centroid_x, centroid_y) + + def increase_layer(self): + centroid = self.calculate_current_layer_centroid() + if centroid: + self.layer_centroids.append(centroid) + self.current_layer += 1 + + def auto_place_block(self): + if self.current_layer == 0: + # 对于第一层，直接在中心放置 + return Block(WIDTH // 2, HEIGHT // 2, 100, 20, 0, self.current_layer) + + base_centroid = self.layer_centroids[0] if self.layer_centroids else (WIDTH // 2, HEIGHT // 2) + current_centroid = self.calculate_current_layer_centroid() or base_centroid + + # 计算当前层的总质量（假设质量与宽度成正比） + total_mass = sum(block.width for block in self.blocks if block.layer == self.current_layer) + + # 假设新积木块的质量与宽度成正比 + new_block_mass = 100 # 新积木块的宽度 + + # 计算理想位置 + ideal_x = (base_centroid[0] * (total_mass + new_block_mass) - current_centroid[0] * total_mass) / new_block_mass + ideal_y = (base_centroid[1] * (total_mass + new_block_mass) - current_centroid[1] * total_mass) / new_block_mass + + # 计算木块中心到塔重心的角度 + dx = base_centroid[0] - ideal_x + dy = base_centroid[1] - ideal_y + center_to_centroid_angle = math.degrees(math.atan2(dy, dx)) + + # 计算垂直于中心线的理想角度 + ideal_angle = (center_to_centroid_angle + 90) % 180 + 90 + + # 步骤 2: 寻找最佳角度 + best_block = None + min_angle_diff = float('inf') + + for angle in range(0, 180, 15): # 每15度尝试一次 + test_block = Block(ideal_x, ideal_y, 100, 20, angle, self.current_layer) + + if not self.check_interference(test_block): + # 检查是否有支撑 + if self.has_support(test_block): + # 计算与理想角度的差异 + angle_diff = min((angle - ideal_angle) % 180, (ideal_angle - angle) % 180) + if angle_diff < min_angle_diff: + min_angle_diff = angle_diff + best_block = test_block + + return best_block + + def has_support(self, block): + if self.current_layer == 0: + return True # 第一层总是有支撑 + + block_corners = block.get_corners() + for lower_block in [b for b in self.blocks if b.layer == self.current_layer - 1]: + lower_corners = lower_block.get_corners() + for corner in block_corners: + if self.point_inside_polygon(corner, lower_corners): + return True + return False + + def auto_place_layer(self): + center_x, center_y = 359, 90 + block_width, block_height = 100, 20 + square_size = 110 # 正方形的边长 + + # 基于当前层数计算旋转角度 + layer_rotation = (self.current_layer * 30) % 360 + + # 计算正方形四个角的坐标 + base_positions = [ + (-square_size/2, -square_size/2), # 左上 + (square_size/2, -square_size/2), # 右上 + (square_size/2, square_size/2), # 右下 + (-square_size/2, square_size/2) # 左下 + ] + + # 旋转基础位置 + rotated_positions = [] + for x, y in base_positions: + angle_rad = math.radians(layer_rotation) + rotated_x = x * math.cos(angle_rad) - y * math.sin(angle_rad) + rotated_y = x * math.sin(angle_rad) + y * math.cos(angle_rad) + rotated_positions.append((center_x + rotated_x, center_y + rotated_y)) + + base_angles = [45, 135, 225, 315] + angles = [(angle - layer_rotation) % 360 for angle in base_angles] + + for (x, y), angle in zip(rotated_positions, angles): + new_block = Block(x, y, block_width, block_height, angle, self.current_layer) + if self.add_block(new_block): + print(f"Block placed at ({x:.2f}, {y:.2f}) with angle {angle}") + else: + print(f"Failed to place block at ({x:.2f}, {y:.2f}) with angle {angle}") + + + def clear_current_layer(self): + self.blocks = [block for block in self.blocks if block.layer!= self.current_layer] + + def scan_current_layer(self): + current_block_pos = cap_and_mark(cap=cap) + for block in current_block_pos: + print(block) + self.add_block_from_robot(block[0][0], block[0][1], block[1]) + + +def main(): + tower = Tower() + clock = pygame.time.Clock() + current_block = None + + running = True + while running: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + running = False + elif event.type == pygame.MOUSEBUTTONDOWN: + if event.button == 1: + x, y = pygame.mouse.get_pos() + current_block = Block(x, y, 100, 20, 0, tower.current_layer) + elif event.type == pygame.MOUSEMOTION: + if current_block: + current_block.x, current_block.y = pygame.mouse.get_pos() + elif event.type == pygame.MOUSEBUTTONUP: + if event.button == 1 and current_block: + if tower.add_block(current_block): + current_block = None + else: + print("CANNOT place block here, there is already a block there.") + elif event.type == pygame.KEYDOWN: + # 处理所有按键事件 + if event.key == pygame.K_r and current_block: # 按 'R' 键旋转当前块 + current_block.angle += 15 + elif event.key == pygame.K_SPACE: # 按 'Space' 键确认当前层完成放置 + tower.increase_layer() + elif event.key == pygame.K_a: # 按 'A' 键自动放置 + auto_block = tower.auto_place_block() + if auto_block: + tower.add_block(auto_block) + else: + print("Cannot find a suitable position for auto-placement.") + elif event.key == pygame.K_n: # 按 'N' 键自动放置新的一层 + tower.auto_place_layer() + elif event.key == pygame.K_s: # 按 'S' 键扫描当前层 + tower.scan_current_layer() + + + screen.fill(WHITE) + tower.draw(screen) + if current_block: + current_block.draw(screen) + + # 计算并绘制当前层的重心 + current_centroid = tower.calculate_current_layer_centroid() + if current_centroid: + pygame.draw.circle(screen, RED, (int(current_centroid[0]), int(current_centroid[1])), 5) + font = pygame.font.Font(None, 24) + + # 绘制之前层的重心 + for i, centroid in enumerate(tower.layer_centroids): + pygame.draw.circle(screen, BLACK, (int(centroid[0]), int(centroid[1])), 3) + font = pygame.font.Font(None, 24) + text = font.render(f"{i+1}", True, BLACK) + screen.blit(text, (int(centroid[0]) + 10, int(centroid[1]) - 10)) + + if not tower.check_stability(): + font = pygame.font.Font(None, 36) + text = font.render("WARNING: Tower is NOT stable", True, RED) + screen.blit(text, (WIDTH // 2 - text.get_width() // 2, 20)) + + font = pygame.font.Font(None, 36) + layer_text = font.render(f"Current layer: {tower.current_layer + 1}", True, BLACK) + screen.blit(layer_text, (10, 10)) + + pygame.display.flip() + clock.tick(60) + + pygame.quit() + +if __name__ == "__main__": + main() diff --git a/manual_control.py b/manual_control.py new file mode 100644 index 0000000..2c834e9 --- /dev/null +++ b/manual_control.py @@ -0,0 +1,70 @@ + +import time + +# 假设jetmax是机械臂的控制模块 +import hiwonder + + +jetmax = hiwonder.JetMax() +sucker = hiwonder.Sucker() + + +import sys +import tty +import termios +import time + +# 假设jetmax是机械臂的控制模块 +# from your_jetmax_library import jetmax + +# 设置初始位置 +position = [-100, -265.94, 80] +step = 5 # 每次移动的步长 +time_to_move = 0.1 # 运动时间 + +# 控制机械臂的位置函数 +def set_position(position, time_to_move): + print(f"Setting position to {position} in {time_to_move} seconds") + # 在这里调用机械臂的接口 + jetmax.set_position(tuple(position), time_to_move) + +# 获取单个字符输入的函数 +def getch(): + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +print("Use WASD to move in the XY plane, R and F to move along Z. Press 'q' to quit.") + +try: + while True: + char = getch() + if char == 'w': + position[1] += step # Y 轴增加 + elif char == 's': + position[1] -= step # Y 轴减少 + elif char == 'a': + position[0] -= step # X 轴减少 + elif char == 'd': + position[0] += step # X 轴增加 + elif char == 'r': + position[2] += step # Z 轴增加 + elif char == 'f': + position[2] -= step # Z 轴减少 + elif char == 'q': + break + + set_position(position, time_to_move) + time.sleep(0.1) + +except KeyboardInterrupt: + print("Program interrupted.") +finally: + print("Program exited.") + + diff --git a/utils/PixelToRealworld b/utils/PixelToRealworld new file mode 160000 index 0000000..cc2fe1b --- /dev/null +++ b/utils/PixelToRealworld @@ -0,0 +1 @@ +Subproject commit cc2fe1bfae20fe2bcb9abfb6c3062af0b995b876 diff --git a/utils/__pycache__/cv_marker.cpython-36.pyc b/utils/__pycache__/cv_marker.cpython-36.pyc new file mode 100644 index 0000000..6bd24a9 Binary files /dev/null and b/utils/__pycache__/cv_marker.cpython-36.pyc differ diff --git a/utils/__pycache__/pixel_convert.cpython-36.pyc b/utils/__pycache__/pixel_convert.cpython-36.pyc new file mode 100644 index 0000000..fed44c3 Binary files /dev/null and b/utils/__pycache__/pixel_convert.cpython-36.pyc differ diff --git a/utils/__pycache__/stack_exe.cpython-36.pyc b/utils/__pycache__/stack_exe.cpython-36.pyc new file mode 100644 index 0000000..8c04fb1 Binary files /dev/null and b/utils/__pycache__/stack_exe.cpython-36.pyc differ diff --git a/utils/cv_marker.py b/utils/cv_marker.py new file mode 100644 index 0000000..5eba027 --- /dev/null +++ b/utils/cv_marker.py @@ -0,0 +1,92 @@ +import cv2 +import math +from utils.pixel_convert import pixel2robot + +def draw_lines_on_image(image): + # 存储线段的起点和终点坐标 + lines = [] + start_point = None + + # 鼠标回调函数，用于捕获鼠标事件 + def draw_line(event, x, y, flags, param): + nonlocal start_point + if event == cv2.EVENT_LBUTTONDOWN: + if start_point is None: + start_point = (x, y) + else: + end_point = (x, y) + lines.append((start_point, end_point)) + cv2.line(image, start_point, end_point, (0, 255, 0), 2) + start_point = None + + # 创建一个窗口 + cv2.namedWindow('Image') + cv2.setMouseCallback('Image', draw_line) + + while True: + cv2.imshow('Image', image) + key = cv2.waitKey(1) & 0xFF + if key == 27: # 按下 'ESC' 键退出 + break + + cv2.destroyAllWindows() + return lines + +def calculate_midpoint_and_angle_2d(start_point, end_point): + # 计算中点坐标 + mid_point = ( + (start_point[0] + end_point[0]) / 2, + (start_point[1] + end_point[1]) / 2 + ) + + # 计算线段的角度（以度为单位） + delta_x = end_point[0] - start_point[0] + delta_y = end_point[1] - start_point[1] + angle = math.degrees(math.atan2(delta_y, delta_x)) + # 将角度值换算到0-180度范围内 + angle = (angle + 180) % 180 + + return mid_point, angle + +def cap_and_mark(cap): + if not cap.isOpened(): + print("无法打开摄像头。") + return + + ret, frame = cap.read() + if not ret: + print("无法获取图像帧。") + return + else: + lines = draw_lines_on_image(frame) + robo_lines = [] + + for line in lines: + start_point, end_point = line + start_point = pixel2robot(start_point[0], start_point[1]) + end_point = pixel2robot(end_point[0], end_point[1]) + # turn array to tuple + start_point = tuple(start_point) + end_point = tuple(end_point) + + mid_point, angle = calculate_midpoint_and_angle_2d(start_point, end_point) + robo_lines.append((mid_point, angle)) + return robo_lines + + +# 测试函数 +if __name__ == "__main__": + # image_path = '1723457093.2643137.jpg' + # image = cv2.imread(image_path) + # if image is None: + # print("图像加载失败！") + # else: + # lines = draw_lines_on_image(image) + # print("绘制的线段坐标:", lines) + cap = cv2.VideoCapture(1) + cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G')) + + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) + lines = cap_and_mark(cap) + print(lines) \ No newline at end of file diff --git a/utils/img_cap.py b/utils/img_cap.py new file mode 100644 index 0000000..475d25c --- /dev/null +++ b/utils/img_cap.py @@ -0,0 +1,67 @@ +import cv2 +import time + +cap = cv2.VideoCapture(1) +cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G')) + +cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) +cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) + +ret, frame = cap.read() +cv2.imwrite(str(time.time()) + '.jpg', frame) + + + +# def SetPoints(windowname, img): +# """ +# 输入图片，打开该图片进行标记点，返回的是标记的几个点的字符串 +# """ +# print('(提示：单击需要标记的坐标，Enter确定，Esc跳过，其它重试。)') +# points = [] + +# def onMouse(event, x, y, flags, param): +# if event == cv2.EVENT_LBUTTONDOWN: +# cv2.circle(temp_img, (x, y), 10, (102, 217, 239), -1) +# points.append([x, y]) +# cv2.imshow(windowname, temp_img) + +# temp_img = img.copy() +# cv2.namedWindow(windowname) +# cv2.imshow(windowname, temp_img) +# cv2.setMouseCallback(windowname, onMouse) +# key = cv2.waitKey(0) +# if key == 13: # Enter +# print('坐标为：', points) +# del temp_img +# cv2.destroyAllWindows() +# return str(points) +# elif key == 27: # ESC +# print('跳过该张图片') +# del temp_img +# cv2.destroyAllWindows() +# return +# else: +# print('重试!') +# return SetPoints(windowname, img) + + + +# SetPoints('test', frame) + + + +# import json +# import requests + +# # Run inference on an image +# url = "https://api.ultralytics.com/v1/predict/MAyxSmPez5SZXdyspDvF" +# headers = {"x-api-key": "803cf64a0603764f0657e33bb0103f8a11ffc59567"} +# data = {"size": 640, "confidence": 0.25, "iou": 0.45} +# with open("test.jpg", "rb") as f: +# response = requests.post(url, headers=headers, data=data, files={"image": f}) + +# # Check for successful response +# response.raise_for_status() + +# # Print inference results +# print(json.dumps(response.json(), indent=2)) \ No newline at end of file diff --git a/utils/location_calib.py b/utils/location_calib.py new file mode 100644 index 0000000..cd77e48 --- /dev/null +++ b/utils/location_calib.py @@ -0,0 +1,65 @@ +''' +完成相机外参矩阵的标定 + +''' + + +import cv2 +import numpy as np + + +# 标定板参数 +pattern_size = (11, 8) # 标定板格点数 +square_size = 0.020 # 每个格子的边长（单位：米） + +# 准备标定板在实际世界中的坐标 +objp = np.zeros((np.prod(pattern_size), 3), dtype=np.float32) +objp[:, :2] = np.indices(pattern_size).T.reshape(-1, 2) +objp *= square_size + +# 存储实际世界中的点坐标和图像中的点坐标 +obj_points = [] +img_points = [] + +# 读取标定板图像 +images = ["1723983285.6120198.jpg"] # 替换为实际的图像文件 + +for fname in images: + img = cv2.imread(fname) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + # 找到棋盘格角点 + ret, corners = cv2.findChessboardCorners(gray, pattern_size) + if ret: + obj_points.append(objp) + img_points.append(corners) + + # 可视化角点 + cv2.drawChessboardCorners(img, pattern_size, corners, ret) + # cv2.imshow('Corners', img) + cv2.waitKey(500) + +cv2.destroyAllWindows() + +# 相机内参和畸变系数（通常先通过标定获取） +camera_matrix = np.array([[1461.09955384437, 0, 963.862411991572], + [0, 1461.89533915958, 545.053909593624], + [0, 0, 1]]) +dist_coeffs = np.array([0.123539164198816, -0.224109122219162, 0, 0, 0]) + +# 使用solvePnP计算外参 +retval, rvec, tvec = cv2.solvePnP(obj_points[0], img_points[0], camera_matrix, dist_coeffs) + +# 将旋转向量转换为旋转矩阵 +R, _ = cv2.Rodrigues(rvec) + +# 平移向量 +T = tvec + +# 打印旋转矩阵和平移向量 +print("旋转矩阵 R:\n", R) +print("平移向量 T:\n", T) + + +# 保存相机参数到文件 +np.savez("camera_params.npz", camera_matrix=camera_matrix, dist_coeffs=dist_coeffs, R=R, T=T) diff --git a/utils/pixel_convert.py b/utils/pixel_convert.py new file mode 100644 index 0000000..c55278d --- /dev/null +++ b/utils/pixel_convert.py @@ -0,0 +1,90 @@ +import numpy as np +import cv2 +import pickle + + + +def pixel2robot(p_x, p_y, Zc=380): + """ + 将像素坐标转换为机器人坐标系坐标 + + 参数： + p_x (int)：像素坐标系x轴坐标 + p_y (int)：像素坐标系y轴坐标 + + 返回值： + robot_coords (numpy.ndarray)：机器人基坐标系下的坐标 + + 示例： + >>> pixel2robot(100, 200) + array([-51.464, -41.334, 774.52]) + """ + + # 摄像头内参矩阵 + camera_matrix = np.array([[1461.09955384437, 0, 963.862411991572], + [0, 1461.89533915958, 545.053909593624], + [0, 0, 1]]) + + # R = np.array([[-0.67004953, -0.74115346, 0.04153518], + # [0.73884813, -0.67127662, -0.05908594], + # [0.07167334, -0.00890232, 0.99738843]]) + # print(R) + + # T = np.array([0.06106497, -0.06272417, 0.7103077]) + + npzfile = np.load('camera_params.npz') + R = npzfile['R'] + T = npzfile['T'] + T = np.reshape(T, (3,)) + + # 336, 174 + u, v = p_x, p_y + + Zc = Zc + + Xc = (u - camera_matrix[0, 2]) * Zc / camera_matrix[0, 0] + Yc = (v - camera_matrix[1, 2]) * Zc / camera_matrix[1, 1] + + # 相机坐标系到标定板坐标系的转换 + camera_coords = np.array([Xc, Yc, Zc]) + base_coords = np.dot(R, camera_coords) + T + # print(base_coords) + + taR = np.array([[1, 0, 0], + [0, -1, 0], + [0, 0, 0]]) + + taT = np.array([-1.5, -216, -30]) + # print(np.dot(taR, base_coords)) + base_coords = np.dot(taR, base_coords) + taT + # base_coords = base_coords + taT + + return base_coords + + +# import cv2 +# import numpy as np + + +# camera_matrix = np.array([[1461.09955384437, 0, 963.862411991572], +# [0, 1461.89533915958, 545.053909593624], +# [0, 0, 1]]) +# dist_coeffs = np.array([0.123539164198816, -0.224109122219162, 0, 0, 0]) + +# def covert(point=[[0.0, 0.0]], z=260): +# point = np.array(point, dtype=np.float) +# pts_uv = cv2.undistortPoints(point, camera_matrix, dist_coeffs) * z +# return pts_uv[0][0] + +# print(covert([993,10],260)) +# print(covert([554,559],260)) + +# 604, 109 [-115, -150.94, -25] + +if __name__ == '__main__': + # print(pixel2robot(468, 254)) + print(pixel2robot(469, 254)) + # print(pixel2robot(956, 448)) + + + diff --git a/utils/stack_exe.py b/utils/stack_exe.py new file mode 100644 index 0000000..7338f48 --- /dev/null +++ b/utils/stack_exe.py @@ -0,0 +1,185 @@ +import time +import hiwonder + +from utils.pixel_convert import pixel2robot + +jetmax = hiwonder.JetMax() +sucker = hiwonder.Sucker() + +pre_x, pre_y, pre_z = 123, -195, 65 +ori_x, ori_y, ori_z = 0, -212, 60 +stack_id = 0 + +STACK_P_X, STACK_P_Y = -110, -8 +STACK_ANGLE = 90 +FIRST_Z = 75 + +PLACE_Z = -10 + +class Stackbot: + def __init__(self): + self.stack_xyz = [STACK_P_X, STACK_P_Y] + self.ori_x, self.ori_y, self.ori_z = 0, -212, 60 + self.first_z = 70 + self.stack_pick_angle = 0 + self.current_level = 0 + + + def pick_stack(self): + stack_xyz = [STACK_P_X, STACK_P_Y] + + jetmax.go_home(1) + time.sleep(1) + + hiwonder.pwm_servo1.set_position(180, 0.1) + time.sleep(0.5) + + jetmax.set_position((stack_xyz[0], stack_xyz[1], FIRST_Z + 100), 1) + time.sleep(2) + + sucker.suck() + jetmax.set_position((stack_xyz[0], stack_xyz[1], FIRST_Z - 8), 2) + + time.sleep(3) + self.stack_pick_angle = hiwonder.serial_servo.read_position(1) + + jetmax.set_position((stack_xyz[0], stack_xyz[1], pre_z + 100), 1) + time.sleep(1) + + jetmax.go_home(1) + time.sleep(1) + + jetmax.go_home(1) + time.sleep(1) + + + + def place_stack(self, x, y, theta, level): + stack_xyz = (x, y, PLACE_Z + level * 12) + + jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2] + 100), 1) + time.sleep(1) + + current_angle = hiwonder.serial_servo.read_position(1) + # hiwonder.pwm_servo1.set_position(180 + (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.1) + current_angle = (current_angle-self.stack_pick_angle) * 0.25 + current_stack_angle = current_angle + STACK_ANGLE + if current_stack_angle > 180: + current_stack_angle = current_stack_angle - 180 + if current_stack_angle > theta: + # print("current_stack_angle > theta") + hiwonder.pwm_servo1.set_position((180 - (180 - (current_stack_angle - theta))), 0.5) + # print(current_stack_angle - theta) + else: + # print("current_stack_angle < theta") + hiwonder.pwm_servo1.set_position(180 - (theta- current_stack_angle), 0.5) + time.sleep(1) + + + + # if stack_id == 6: + # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 + 30, 0.5) + # else: + # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.5) + + # hiwonder.pwm_servo1.set_position(180 - theta, 0.5) + # time.sleep(1) + + jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2]), 2) + time.sleep(2) + + + + sucker.release() + + def place_stack_classic(self, px, py, theta, level): + stack_xyz = pixel2robot(px, py) + print(stack_xyz) + stack_xyz = (stack_xyz[0], stack_xyz[1], PLACE_Z + level * 12) + + jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2] + 100), 1) + time.sleep(1) + + current_angle = hiwonder.serial_servo.read_position(1) + # hiwonder.pwm_servo1.set_position(180 + (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.1) + print(current_angle, self.stack_pick_angle) + time.sleep(5) + + + + # if stack_id == 6: + # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 + 30, 0.5) + # else: + # hiwonder.pwm_servo1.set_position(180 - (current_angle-self.stack_pick_angle) * 0.25 - theta, 0.5) + + hiwonder.pwm_servo1.set_position(180 - theta, 0.5) + time.sleep(1) + + jetmax.set_position((stack_xyz[0], stack_xyz[1], stack_xyz[2] + 10), 2) + time.sleep(2) + + + + sucker.release() + + +if __name__ == '__main__': + # jetmax.go_home(1) + # time.sleep(1) + + # jetmax.set_position((ori_x, ori_y, 200), 1) + hiwonder.pwm_servo1.set_position(180, 0.1) + # pick_stack(1) + sucker.release() + stackbot = Stackbot() + stackbot.pick_stack() + stackbot.place_stack(-100, -100, 90, 10) + + # stackbot.pick_stack(2) + # stackbot.place_stack(2, 172, 217, 150, 0) + + # stackbot.pick_stack(3) + # stackbot.place_stack(3, 300, 430, 90, 0) + + # stackbot.pick_stack(4) + # stackbot.place_stack(4, 274, 120, 90, 1) + + # stackbot.pick_stack(5) + # stackbot.place_stack(5, 180, 373, 30, 1) + + # stackbot.pick_stack(6) + # stackbot.place_stack(6, 412, 333, 150, 1) + + # while True: + # ori_z += 16 + # pick_stack(stack_id) + + + # jetmax.set_position((ori_x, ori_y, ori_z), 1) + # time.sleep(1) + + # sucker.release() + # time.sleep(2) + + # stack_id += 1 + + # while True: + # jetmax.set_position((cur_x-100, cur_y, cur_z), 1) + # time.sleep(2) + # jetmax.set_position((cur_x+100, cur_y, cur_z), 1) + # time.sleep(2) + # jetmax.set_position((cur_x, cur_y, cur_z), 1) + # time.sleep(2) + + # jetmax.set_position((cur_x, cur_y-50, cur_z), 1) + # time.sleep(2) + # jetmax.set_position((cur_x, cur_y+50, cur_z), 1) + # time.sleep(2) + # jetmax.set_position((cur_x, cur_y, cur_z), 1) + # time.sleep(2) + + # jetmax.set_position((cur_x, cur_y, cur_z-100), 1) + # time.sleep(2) + # jetmax.set_position((cur_x, cur_y, cur_z), 0.5) + # time.sleep(2) +