init
This commit is contained in:
raiot
commit
25e83f05db
|
|
@ -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)
|
||||
|
|
@ -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()
|
||||
|
|
@ -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.")
|
||||
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit cc2fe1bfae20fe2bcb9abfb6c3062af0b995b876
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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)
|
||||
|
|
@ -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))
|
||||
|
|
@ -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)
|
||||
|
|
@ -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))
|
||||
|
||||
|
||||
|
||||
|
|
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue