LU/multi-model-optimizer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改数据格式和双面板优化

Browse Source
This commit is contained in:
hit_lu
2025-12-09 09:29:10 +08:00
parent 96e4b3d856
commit d627cf9f6e
8 changed files with 125 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
opt/smm/path_plan.py
View File

@@ -1,4 +1,5 @@
import copy
import math
import numpy as np
@@ -91,86 +92,80 @@ class PathPlanOpt:
head_sequence = list(reversed(head_sequence))
return ans_dist, head_sequence
def scan_based(self, part_result, cycle_result, slot_result):
def scan_based(self, part_result, cycle_result, slot_result, hinter=True):
point_result, sequence_result = [], []
class Mount:
def __init__(self):
self.pos = []
self.angle = []
self.part = []
self.step = []
def pop(self, index):
self.pos.pop(index)
self.angle.pop(index)
self.part.pop(index)
self.step.pop(index)
all_points = Mount()
for step_index, data in self.step_data.iterrows():
part_index = self.part_data[self.part_data.part == data.part].index.tolist()[0]
# <20><>¼<EFBFBD><C2BC>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD>Ӧ<EFBFBD><D3A6>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
all_points.pos.append(Point(data.x + self.config.stopper_pos.x, data.y + self.config.stopper_pos.y))
all_points.angle.append(data.r)
all_points.pos.append(Point(data.x + self.config.stopper_pos.x, data.y + self.config.stopper_pos.y, data.r))
all_points.part.append(part_index)
all_points.step.append(step_index)
point_list = list(range(0, self.step_data.shape[0]))
head_num = self.config.head_num
left_boundary, right_boundary = min(all_points.pos, key=lambda p: p.x).x, \
max(all_points.pos, key=lambda p: p.x).x
search_step = max((right_boundary - left_boundary) / head_num / 2, 0)
search_points, head_range = defaultdict(lambda: np.array([])), defaultdict(list)
search_points[0] = np.arange(left_boundary, (left_boundary + right_boundary) / 2, search_step)
head_range[0] = list(range(head_num))
search_points[1] = np.arange(right_boundary + 1e-3, (left_boundary + right_boundary) / 2, -search_step)
head_range[1] = list(range(head_num - 1, -1, -1))
search_points[2] = np.arange(left_boundary, right_boundary, search_step)
head_range[2], head_index = [], (head_num - 1) // 2
while head_index >= 0:
if 2 * head_index != head_num - 1:
head_range[2].append(head_num - 1 - head_index)
head_range[2].append(head_index)
head_index -= 1
pbar = tqdm(total=sum(cycle_result), desc='scan-based path plan process') if hinter else None
ref_pos_y = min(all_points.pos, key=lambda p: p.y).y
for cycle_index, component_cycle in enumerate(part_result):
for _ in range(cycle_result[cycle_index]):
min_dist = np.inf
tmp_assigned_point, tmp_assigned_head_seq = [], []
best_head_point, best_head_seq, best_point_list = [], [], []
for dir in range(3): # <20><>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װͷ<D7B0><CDB7><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ѡȡ<D1A1><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ
for start_points in search_points[dir]:
cur_point_list = point_list.copy()
tmp_all_points = Mount()
for search_dir in range(3): # <20><>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װͷ<D7B0><CDB7><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ѡȡ<D1A1><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ
if search_dir == 0:
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
search_points = np.arange(left_boundary, (left_boundary + right_boundary) / 2, search_step)
head_range = list(range(head_num))
elif search_dir == 1:
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
search_points = np.arange(right_boundary + 1e-3, (left_boundary + right_boundary) / 2, -search_step)
head_range = list(range(head_num - 1, -1, -1))
else:
# <20><><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
search_points = np.arange(left_boundary, right_boundary, search_step / 2)
head_range, head_index = [], (head_num - 1) // 2
while head_index >= 0:
if 2 * head_index != head_num - 1:
head_range.append(head_num - 1 - head_index)
head_range.append(head_index)
head_index -= 1
for start_points in search_points:
cur_all_points = copy.deepcopy(all_points)
assigned_point = [-1] * head_num
assigned_mount_point, assigned_mount_angle = [Point(0, 0)] * head_num, [0] * head_num
head_point = [-1] * head_num
assigned_point = [Point(0, 0)] * head_num
head_counter, point_index = 0, -1
for head_index in head_range:
for head_index in head_range[dir]:
if head_counter == 0:
part_index = part_result[cycle_index][head_index]
if part_index == -1:
if part_result[cycle_index][head_index] == -1:
continue
min_horizontal_distance = np.inf
for index, part in enumerate(cur_all_points.part):
if part != part_result[cycle_index][head_index]:
for index, point in enumerate(cur_point_list):
if all_points.part[point] != part_result[cycle_index][head_index]:
continue
horizontal_distance = abs(cur_all_points.pos[index].x - start_points) + 0 * abs(
cur_all_points.pos[index].y - ref_pos_y)
horizontal_distance = abs(all_points.pos[point].x - start_points) + 0 * abs(
all_points.pos[point].y - ref_pos_y)
if horizontal_distance < min_horizontal_distance:
min_horizontal_distance = horizontal_distance
@@ -179,18 +174,18 @@ class PathPlanOpt:
point_index = -1
min_cheby_distance = np.inf
for index, part in enumerate(cur_all_points.part):
if part != part_result[cycle_index][head_index]:
for index, point in enumerate(cur_point_list):
if all_points.part[point] != part_result[cycle_index][head_index]:
continue
point_pos = [Point(cur_all_points.pos[index].x - head_index * self.config.head_intv,
cur_all_points.pos[index].y)]
point_pos = [Point(all_points.pos[point].x - head_index * self.config.head_intv,
all_points.pos[point].y)]
cheby_distance, euler_distance = 0, 0
for next_head in range(head_num):
if assigned_point[next_head] == -1:
if head_point[next_head] == -1:
continue
point_pos.append(Point(assigned_mount_point[next_head].x - next_head * head_num,
assigned_mount_point[next_head].y))
point_pos.append(Point(assigned_point[next_head].x - next_head * head_num,
assigned_point[next_head].y))
point_pos = sorted(point_pos, key=lambda p: p.x)
for mount_seq in range(len(point_pos) - 1):
@@ -211,22 +206,22 @@ class PathPlanOpt:
head_counter += 1
assigned_point[head_index] = all_points.step[point_index]
assigned_mount_point[head_index] = all_points.pos[point_index]
assigned_mount_angle[head_index] = all_points.angle[point_index]
step_index = cur_point_list[point_index]
head_point[head_index] = all_points.step[step_index]
assigned_point[head_index] = all_points.pos[step_index]
cur_all_points.pop(point_index)
cur_point_list.pop(point_index)
dist, head_seq = self.dynamic_programming_cycle_path(assigned_point, slot_result[cycle_index])
dist, head_seq = self.dynamic_programming_cycle_path(head_point, slot_result[cycle_index])
if min_dist is None or dist < min_dist:
tmp_all_points = cur_all_points
tmp_assigned_point, tmp_assigned_head_seq = assigned_point, head_seq
if dist < min_dist:
best_head_point, best_head_seq, best_point_list = head_point, head_seq, cur_point_list
min_dist = dist
all_points = tmp_all_points
point_result.append(tmp_assigned_point)
sequence_result.append(tmp_assigned_head_seq)
if pbar:
pbar.update(1)
point_list = best_point_list
point_result.append(best_head_point)
sequence_result.append(best_head_seq)
return point_result, sequence_result