From 25e83f05dbf4a30768fd41ce636184420f946e42 Mon Sep 17 00:00:00 2001 From: raiot Date: Wed, 28 Aug 2024 13:48:46 +0800 Subject: [PATCH] init --- arm_debug.py | 29 ++ game_plat.py | 430 ++++++++++++++++++ manual_control.py | 70 +++ utils/PixelToRealworld | 1 + utils/__pycache__/cv_marker.cpython-36.pyc | Bin 0 -> 1956 bytes .../__pycache__/pixel_convert.cpython-36.pyc | Bin 0 -> 1290 bytes utils/__pycache__/stack_exe.cpython-36.pyc | Bin 0 -> 2783 bytes utils/cv_marker.py | 92 ++++ utils/img_cap.py | 67 +++ utils/location_calib.py | 65 +++ utils/pixel_convert.py | 90 ++++ utils/stack_exe.py | 185 ++++++++ 12 files changed, 1029 insertions(+) create mode 100644 arm_debug.py create mode 100644 game_plat.py create mode 100644 manual_control.py create mode 160000 utils/PixelToRealworld create mode 100644 utils/__pycache__/cv_marker.cpython-36.pyc create mode 100644 utils/__pycache__/pixel_convert.cpython-36.pyc create mode 100644 utils/__pycache__/stack_exe.cpython-36.pyc create mode 100644 utils/cv_marker.py create mode 100644 utils/img_cap.py create mode 100644 utils/location_calib.py create mode 100644 utils/pixel_convert.py create mode 100644 utils/stack_exe.py 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 0000000000000000000000000000000000000000..6bd24a9dc57055633f4778f9c3e5c1076fb52b54 GIT binary patch literal 1956 zcmZ8i-D_M$6rY*PfLKM;2VX=4(H9>q=tJ=jnb(qF{smw3oY^!9F3g#kbI!~?=QqD|_R7de zsqy3U)^8<3UXWA20<5pY$i4)@38yjfhcopl>{-k@mTytQEp9(1zRex(!tC$@_h5EK zffqNJ{9KT>$4dk17e%pM;$_&E+ZA5nBYc#P4eSA<1TCN+;nh>SQ9f>Vt|-FkF;Nv` z0}9`xE2K7Y2vL#68r9`)v@POknLbEWlcE-W)~S(k*sPH~2zrk_Ay4QN*0MO|?6KFU zinJ-W@LE_gZsim|x&1EeEN<-*+`-!B_JDHdI_a~VaTn6Myi6WmZlKK>5K4A2sKR2? z!LP&5g@Fj3fsx$-k)tbxgY&zLlMPQ^%xTVAY>y@H4Oe|5g_6NJ(JD)ib%3l*0ZLJx+<%$sIa+lBUSaDisG!^+zdLQ z+z@iAyQ7O-hFbyNUWP2PQ4oZcs7Fi8qoZ_!e)`U_M;4?>VB(p&8mG-L&aO=z8-3?C zMtX=48AJ{*z^dj-d>p@=gdM>fQNq(LznTfPkoGb$8^-Yih|PDSPPPt~!d4imkHn5Y z&PAqVx-%2U!$TQ>J%U$HfY8>4*pXGd|455*%P|z=Xg_lt)=A75M|KVbVK%j`YNbRg zjL`~Y=}{N5m6Kp=;=|$(xn#qbg^?v7a;SmxB&R4}g-X1D8p~)v4mB+XR5cz$?{lON zLV z=5N8hLvmOXbkeTUDxF{zCSM2ras5LxBZ#tr>{nRiq)i4SJF5T^(?p<*K*t8;KI>D2 zD!U9eQ+QZn%E}QCYYkx9=DBr~d~zS*0dKcoPQrgYSMg`2CxMy|4G5KRWp8 z@&4fH{!hT3rj35c@YH`!Krtw zz)j}iu)?o^EqfaT0fu|jW)*6i8P{0j6Yz*&VOZx99kmxgZ-|m07*Y=7({w@84>euZ z^j7o;0*T%SPS(avVM_$fKVurpl=)8xnrX5rq%z44(>AHTydE1t5a`Oih>LVK?5ds= z@=g4X1#Ll>jlLlxC8Ty+X-_tr`t0n?a<73s#N6!iL{OPue6718;X#)z@gg%7*&?*akK6wwi!Fi zrm`g<6&xxkhf<01S0N4{^-zf(s?Z;C|Akdh%$l}c2;iVfLOsIp1eDJSE| zG()pp(#06E)=@P#0tWQjD8>lc64zl(!_L5d33m7gK!h%1iYcK|LjshkA|#=i26(zS zksyKqF;3)Irs_OO6p%TP890&R44v+hBom_;6O?2VH6{s(v#~@=^JosmWEO!wGmnm< zBp0ioADZwV7JiLa5$9qpCgRRK*mA7I`7VhwM^IAW=kR%i!a3mVz7)2|D2j+@=(F^Q zu%`q=F)jeZ6laJF(2}|KE6@zB3~O-r{XzHo&b`}%FFqdL{$Tgvo#BUH_wVax()RAdd;5v}w!rNCUu5SX zQjqRGzA^ah%fUMj#fD_*-5bZQ>87_&P3;)8;}I;gpS|Ju&2tCN-Q zeADYcx;ExqTU#p!?zYoh7VMR!#l?m4^5SAulpwe(ZyrCfR9k&vb)kHGb*Z+p0?Nsg ztF;r$7pi3u*3cSn{q6j6v(BKYsPW|J*;B(~AJrSf{!c&OcyhDeU<=Q0cib=x3s9K`R<%gA4}LT7(?h7?)nIGq0uu|tsh z@5}mz-*WT~_if*!4%3+*>LIiBb;k>RriV6jgDBM3Bg@`A>qmM!a+{$p1_zwpmcyc2 z&`I+XqvRb5p94TM1+3z!oCc`kf}|0d%;7nygiDe*Ath3h*epb*s%f??*E3D`4s_2w z1FeO|^O01usUn^n%Y$V|_SSNm30!*tg#HCkg=q2f+sk6hMe$q9lR@E5a#Y6lrzbjGbNYJ(yi5 z=i^*m{sKe;qM|^er=g-qk0_|NLZaasN`z>_?>Fn)a}rDJX}Rp4oyudxN z2&Ez~@v}p7 z<2&K-S`iU-nyls^ngS0KgbK_tPC=S)z+9ujT{Lzs}8!V)&*mT-g% zxh)E?94~dZl4ffc*3hER86V6|NLL}nuY)8cp*w6yd?xI>2IQDbNR~jg1qF4qx`M!{ z&Zfdal#SwmjIX1dfBg7y-N;_eOkULsX<_Tm#?_ntZU2*L@zb;r$YX% zUhtsX5mGf&H*UnL)!34qxT_j*O9j0oZs>h^zne7nk{|*Ly&Bnru$pER(s#$E9 z14->rChcaQJ3Cz;9hsU*0BXgNl)coQxHa{9588e#)&1@pX(^V8-|NOf5_CJMbNz#l zx9<2nwd1|ku2kH_9F0`ajIusyS;?lz`=~{n+UhCk59&-U;cDhlkf=;*XyIuJH7l!7 zkI#Y->N1adtODsYB$u9sdPSG9-J^qr9GPkV7J`Bgp8gz~KcOR&-mFJuruz+z}D6Z{DmA?hqn-VKc*^qI6L(^-#QAwXzq2+=NsyBL-bT8&F# zb6kS&%_7E0ES*L<1#$?@uj>l;IY+2oPMzDgHm+XtxjKWkFQPn$g04`{qr8k#N6}s_ zX5nXJ^Tw@f>IH0m2_$utjAS#GbyvGPrTvsb@Ja5;q?uYevSVUjSw$H?)4?LYb!%2Rw`j0_sVG(HfM!UC2LY#12<@^TCN(4q+L z)QDtwAaaMjkDm#!%TYkv30EhuP6*7cxr=AqFzI* zGbsPh#1sS7fF5;6Bfwd&pR(S*x`3lxMA0$eO)NbX2o{cb*l$J6I1XAq*u^U#cv-k~ z<J{d}@8ElLuzH-ACj)HO9lUxv&9?OY)bsszSL{VtF8TiFd(9|&qi_!OD#}|Z zNZ66}>6K!%Qe}_@NHK;OK+bhfx#x5%PSb7%9pBG3b`q=g*&Dx?UE~;=)nyd)z!TT| zI<_K{<1fK>{10|u>I$}QfCPU-L-6fH>Z{oi#nYKC==*j}|2y(q-Ohcf675KCe<+iW zntkm=Zg1(>t>> 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) +