auto
This commit is contained in:
raiot
parent
25e83f05db
commit
53ccf4439c
|
|
@ -0,0 +1,176 @@
|
|||
# Created by https://www.toptal.com/developers/gitignore/api/python
|
||||
# Edit at https://www.toptal.com/developers/gitignore?templates=python
|
||||
|
||||
### Python ###
|
||||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.so
|
||||
|
||||
# Distribution / packaging
|
||||
.Python
|
||||
build/
|
||||
develop-eggs/
|
||||
dist/
|
||||
downloads/
|
||||
eggs/
|
||||
.eggs/
|
||||
lib/
|
||||
lib64/
|
||||
parts/
|
||||
sdist/
|
||||
var/
|
||||
wheels/
|
||||
share/python-wheels/
|
||||
*.egg-info/
|
||||
.installed.cfg
|
||||
*.egg
|
||||
MANIFEST
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
*.manifest
|
||||
*.spec
|
||||
|
||||
# Installer logs
|
||||
pip-log.txt
|
||||
pip-delete-this-directory.txt
|
||||
|
||||
# Unit test / coverage reports
|
||||
htmlcov/
|
||||
.tox/
|
||||
.nox/
|
||||
.coverage
|
||||
.coverage.*
|
||||
.cache
|
||||
nosetests.xml
|
||||
coverage.xml
|
||||
*.cover
|
||||
*.py,cover
|
||||
.hypothesis/
|
||||
.pytest_cache/
|
||||
cover/
|
||||
|
||||
# Translations
|
||||
*.mo
|
||||
*.pot
|
||||
|
||||
# Django stuff:
|
||||
*.log
|
||||
local_settings.py
|
||||
db.sqlite3
|
||||
db.sqlite3-journal
|
||||
|
||||
# Flask stuff:
|
||||
instance/
|
||||
.webassets-cache
|
||||
|
||||
# Scrapy stuff:
|
||||
.scrapy
|
||||
|
||||
# Sphinx documentation
|
||||
docs/_build/
|
||||
|
||||
# PyBuilder
|
||||
.pybuilder/
|
||||
target/
|
||||
|
||||
# Jupyter Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
# IPython
|
||||
profile_default/
|
||||
ipython_config.py
|
||||
|
||||
# pyenv
|
||||
# For a library or package, you might want to ignore these files since the code is
|
||||
# intended to run in multiple environments; otherwise, check them in:
|
||||
# .python-version
|
||||
|
||||
# pipenv
|
||||
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
|
||||
# However, in case of collaboration, if having platform-specific dependencies or dependencies
|
||||
# having no cross-platform support, pipenv may install dependencies that don't work, or not
|
||||
# install all needed dependencies.
|
||||
#Pipfile.lock
|
||||
|
||||
# poetry
|
||||
# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
|
||||
# This is especially recommended for binary packages to ensure reproducibility, and is more
|
||||
# commonly ignored for libraries.
|
||||
# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
|
||||
#poetry.lock
|
||||
|
||||
# pdm
|
||||
# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
|
||||
#pdm.lock
|
||||
# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
|
||||
# in version control.
|
||||
# https://pdm.fming.dev/#use-with-ide
|
||||
.pdm.toml
|
||||
|
||||
# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
|
||||
__pypackages__/
|
||||
|
||||
# Celery stuff
|
||||
celerybeat-schedule
|
||||
celerybeat.pid
|
||||
|
||||
# SageMath parsed files
|
||||
*.sage.py
|
||||
|
||||
# Environments
|
||||
.env
|
||||
.venv
|
||||
env/
|
||||
venv/
|
||||
ENV/
|
||||
env.bak/
|
||||
venv.bak/
|
||||
|
||||
# Spyder project settings
|
||||
.spyderproject
|
||||
.spyproject
|
||||
|
||||
# Rope project settings
|
||||
.ropeproject
|
||||
|
||||
# mkdocs documentation
|
||||
/site
|
||||
|
||||
# mypy
|
||||
.mypy_cache/
|
||||
.dmypy.json
|
||||
dmypy.json
|
||||
|
||||
# Pyre type checker
|
||||
.pyre/
|
||||
|
||||
# pytype static type analyzer
|
||||
.pytype/
|
||||
|
||||
# Cython debug symbols
|
||||
cython_debug/
|
||||
|
||||
# PyCharm
|
||||
# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
|
||||
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
|
||||
# and can be added to the global gitignore or merged into this file. For a more nuclear
|
||||
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
|
||||
#.idea/
|
||||
|
||||
### Python Patch ###
|
||||
# Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration
|
||||
poetry.toml
|
||||
|
||||
# ruff
|
||||
.ruff_cache/
|
||||
|
||||
# LSP config files
|
||||
pyrightconfig.json
|
||||
|
||||
# End of https://www.toptal.com/developers/gitignore/api/python
|
||||
154
game_plat.py
154
game_plat.py
|
|
@ -3,7 +3,7 @@ import math
|
|||
import numpy as np
|
||||
import random
|
||||
from utils.cv_marker import cap_and_mark
|
||||
from utils.stack_exe import Stackbot
|
||||
# from utils.stack_exe import Stackbot
|
||||
import cv2
|
||||
|
||||
|
||||
|
|
@ -79,10 +79,13 @@ class Tower:
|
|||
])
|
||||
self.robot_to_sim_matrix = np.linalg.inv(self.sim_to_robot_matrix)
|
||||
|
||||
self.stack_bot = Stackbot()
|
||||
# self.stack_bot = Stackbot()
|
||||
|
||||
self.is_upper = False
|
||||
|
||||
self.ideal_positions = self.get_ideal_positions()
|
||||
self.position_tolerance = 20 # 允许的位置偏差（像素）
|
||||
self.angle_tolerance = 15 # 允许的角度偏差（度）
|
||||
|
||||
def sim_to_robot_coords(self, x, y, angle):
|
||||
sim_coords = np.array([x, y, 0, 1])
|
||||
|
|
@ -106,7 +109,7 @@ class Tower:
|
|||
angle (float): 机器人的角度
|
||||
|
||||
返回值：
|
||||
bool：如果方块添加成功，返回 True，否则返回 False
|
||||
bool：如果方块添加成，返回 True，否则返回 False
|
||||
|
||||
注意：这个函数需要先调用 robot_to_sim_coords 来将机器人坐标转换为模拟坐标，并且需要一个名为 Block 的类来创建方块对象
|
||||
"""
|
||||
|
|
@ -121,21 +124,27 @@ class Tower:
|
|||
|
||||
|
||||
def add_block(self, block):
|
||||
if self.current_layer == 5:
|
||||
self.is_upper = True
|
||||
print("is upper")
|
||||
self.add_block_from_sim(block)
|
||||
return True
|
||||
# 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
|
||||
# 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 add_block_from_sim(self, block):
|
||||
self.blocks.append(block)
|
||||
return True
|
||||
|
||||
def draw(self, surface):
|
||||
for block in self.blocks:
|
||||
|
|
@ -252,49 +261,85 @@ class Tower:
|
|||
self.layer_centroids.append(centroid)
|
||||
self.current_layer += 1
|
||||
|
||||
def get_ideal_positions(self):
|
||||
# 定义中心原点
|
||||
center_x, center_y = 400, 450
|
||||
|
||||
# 定义相对坐标和角度
|
||||
relative_positions = [
|
||||
[(0, 40, 0), (0, -40, 0)], # 第一层
|
||||
[(-40, 0, 90), (40, 0, 90)], # 第二层
|
||||
[(0, 40, 0), (0, -40, 0)], # 第三层
|
||||
[(-40, 0, 90), (40, 0, 90)], # 第四层
|
||||
[(0, 40, 0), (0, -40, 0)], # 第五层
|
||||
# ... 可以继续添加更多层 ...
|
||||
]
|
||||
|
||||
# 转换为绝对坐标
|
||||
absolute_positions = []
|
||||
for layer in relative_positions:
|
||||
layer_positions = []
|
||||
for rel_x, rel_y, angle in layer:
|
||||
abs_x = center_x + rel_x
|
||||
abs_y = center_y + rel_y
|
||||
layer_positions.append((abs_x, abs_y, angle))
|
||||
absolute_positions.append(layer_positions)
|
||||
|
||||
return absolute_positions
|
||||
|
||||
def auto_place_block(self):
|
||||
if self.current_layer == 0:
|
||||
# 对于第一层，直接在中心放置
|
||||
return Block(WIDTH // 2, HEIGHT // 2, 100, 20, 0, self.current_layer)
|
||||
if self.current_layer >= len(self.ideal_positions):
|
||||
print("塔已经完成")
|
||||
return None
|
||||
|
||||
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
|
||||
possible_positions = self.generate_possible_positions()
|
||||
best_position = self.find_best_position(possible_positions)
|
||||
|
||||
# 计算当前层的总质量（假设质量与宽度成正比）
|
||||
total_mass = sum(block.width for block in self.blocks if block.layer == self.current_layer)
|
||||
|
||||
# 假设新积木块的质量与宽度成正比
|
||||
new_block_mass = 100 # 新积木块的宽度
|
||||
if best_position:
|
||||
x, y, angle = best_position
|
||||
print(f"选择的最佳位置: ({x}, {y}) 角度为 {angle}") # 添加这行来调试
|
||||
return Block(x, y, 100, 20, angle, self.current_layer)
|
||||
else:
|
||||
print("无法找到合适的放置位置")
|
||||
return None
|
||||
|
||||
# 计算理想位置
|
||||
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
|
||||
def generate_possible_positions(self):
|
||||
ideal_positions = self.ideal_positions[self.current_layer]
|
||||
possible_positions = []
|
||||
|
||||
# 计算木块中心到塔重心的角度
|
||||
dx = base_centroid[0] - ideal_x
|
||||
dy = base_centroid[1] - ideal_y
|
||||
center_to_centroid_angle = math.degrees(math.atan2(dy, dx))
|
||||
for ideal_x, ideal_y, ideal_angle in ideal_positions:
|
||||
for dx in range(-self.position_tolerance, self.position_tolerance + 1, 2):
|
||||
for dy in range(-self.position_tolerance, self.position_tolerance + 1, 2):
|
||||
for dangle in range(-self.angle_tolerance, self.angle_tolerance + 1, 5):
|
||||
x = ideal_x + dx
|
||||
y = ideal_y + dy
|
||||
angle = (ideal_angle + dangle) % 360
|
||||
possible_positions.append((x, y, angle))
|
||||
|
||||
# 计算垂直于中心线的理想角度
|
||||
ideal_angle = (center_to_centroid_angle + 90) % 180 + 90
|
||||
print(f"生成的可能位置数量: {len(possible_positions)}")
|
||||
return possible_positions
|
||||
|
||||
# 步骤 2: 寻找最佳角度
|
||||
best_block = None
|
||||
min_angle_diff = float('inf')
|
||||
def find_best_position(self, possible_positions):
|
||||
best_position = None
|
||||
min_distance = float('inf')
|
||||
|
||||
for angle in range(0, 180, 15): # 每15度尝试一次
|
||||
test_block = Block(ideal_x, ideal_y, 100, 20, angle, self.current_layer)
|
||||
for x, y, angle in possible_positions:
|
||||
new_block = Block(x, 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
|
||||
if self.has_support(new_block) and not self.check_interference(new_block):
|
||||
temp_tower = Tower()
|
||||
temp_tower.blocks = self.blocks.copy()
|
||||
temp_tower.add_block(new_block)
|
||||
if temp_tower.check_stability():
|
||||
ideal_x, ideal_y, ideal_angle = self.ideal_positions[self.current_layer][len(self.blocks) % 2]
|
||||
distance = np.sqrt((x - ideal_x)**2 + (y - ideal_y)**2) + abs(angle - ideal_angle)
|
||||
|
||||
if distance < min_distance:
|
||||
min_distance = distance
|
||||
best_position = (x, y, angle)
|
||||
|
||||
return best_block
|
||||
print(f"选择的最佳位置: {best_position}，距离理想位置的距离: {min_distance}")
|
||||
return best_position
|
||||
|
||||
def has_support(self, block):
|
||||
if self.current_layer == 0:
|
||||
|
|
@ -372,7 +417,7 @@ def main():
|
|||
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):
|
||||
if tower.add_block_from_sim(current_block):
|
||||
current_block = None
|
||||
else:
|
||||
print("CANNOT place block here, there is already a block there.")
|
||||
|
|
@ -385,9 +430,12 @@ def main():
|
|||
elif event.key == pygame.K_a: # 按 'A' 键自动放置
|
||||
auto_block = tower.auto_place_block()
|
||||
if auto_block:
|
||||
tower.add_block(auto_block)
|
||||
if tower.add_block_from_sim(auto_block):
|
||||
print(f"成功放置积木在 ({auto_block.x}, {auto_block.y}) 角度为 {auto_block.angle}")
|
||||
else:
|
||||
print("无法放置积木")
|
||||
else:
|
||||
print("Cannot find a suitable position for auto-placement.")
|
||||
print("无法找到合适的放置位置")
|
||||
elif event.key == pygame.K_n: # 按 'N' 键自动放置新的一层
|
||||
tower.auto_place_layer()
|
||||
elif event.key == pygame.K_s: # 按 'S' 键扫描当前层
|
||||
|
|
|
|||
Loading…
Reference in New Issue