diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..90f8fbb --- /dev/null +++ b/.gitignore @@ -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 \ No newline at end of file diff --git a/game_plat.py b/game_plat.py index 6ec8695..f05ed00 100644 --- a/game_plat.py +++ b/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' 键扫描当前层