def get_duals(drive_model: Model, x_sample_count: int, y_sample_count: int,
horizontal_shifting: float):
"""
Get duals of a given drive model, self-creating debugger
:param drive_model: the driving model
:param x_sample_count: count of samples in x direction
:param y_sample_count: count of samples in y direction
:param horizontal_shifting: shifting in x direction to keep the drive away from input
:return: None
"""
debugger, _, plotter = initialize((drive_model, ), None, ['duals'])
drive_contour = shape_factory.get_shape_contour(drive_model, True, None,
drive_model.smooth)
logging.debug('drive model loaded')
# get the bounding
drive_polygon = Polygon(drive_contour)
min_x, min_y, max_x, max_y = drive_polygon.bounds
drive_windows = [(min_x, max_x, min_y, max_y)]
drive_windows = split_window(drive_windows[0], x_sample_count,
y_sample_count)
centers = [center_of_window(window) for window in drive_windows]
# start finding the dual
for index, center in enumerate(centers):
if not point_in_contour(drive_contour, *center):
logging.info(f'Point #{index}{center} not in contour')
continue
drive_polar = toExteriorPolarCoord(Point(*center), drive_contour, 1024)
driven_polar, center_distance, phi = compute_dual_gear(drive_polar)
drive_new_contour = toCartesianCoordAsNp(drive_polar,
horizontal_shifting, 0)
driven_contour = toCartesianCoordAsNp(
driven_polar, horizontal_shifting + center_distance, 0)
driven_contour = np.array(
rotate(driven_contour, phi[0],
(horizontal_shifting + center_distance, 0)))
# move things back to center
drive_new_contour += np.array((center[0], center[1]))
driven_contour += np.array((center[0], center[1]))
plotter.draw_contours(
debugger.file_path(f'{index}.png'),
[('input_drive', drive_contour), ('math_drive', drive_new_contour),
('math_driven', driven_contour)],
[(horizontal_shifting + center[0], center[1]),
(horizontal_shifting + center_distance + center[0], center[1])])
def plot_polar_shape(ax, title, polar_contour, center, sample_num):
cartesian_contour = toCartesianCoordAsNp(polar_contour, center[0],
center[1])
ax.set_title(title)
ax.fill(cartesian_contour[:, 0], cartesian_contour[:, 1], "g", alpha=0.3)
for p in cartesian_contour[1:-1:int(len(cartesian_contour) / 32)]:
l = Line2D([center[0], p[0]], [center[1], p[1]], linewidth=1)
ax.add_line(l)
ax.scatter(center[0], center[1], s=10, c='b')
ax.axis('equal')
def math_cut(drive_model: Model,
cart_drive: np.ndarray,
reporter: Reporter,
plotter: Optional[Plotter],
animation=False,
center_point: Optional[Tuple[float, float]] = None):
center = center_point or drive_model.center_point
polar_math_drive = toExteriorPolarCoord(Point(center[0], center[1]),
cart_drive, drive_model.sample_num)
polar_math_driven, center_distance, phi = compute_dual_gear(
polar_math_drive, k=drive_model.k)
if animation:
plot_sampled_function((polar_math_drive, polar_math_driven), (phi, ),
reporter.get_math_debug_dir_name(),
100,
0.001, [(0, 0), (center_distance, 0)], (8, 8),
((-0.5, 1.5), (-1.1, 1.1)),
plotter=plotter)
# save figures
plotter.draw_contours(
reporter.file_path('math_drive.png'),
[('math_drive', toCartesianCoordAsNp(polar_math_drive, 0, 0))], None)
plotter.draw_contours(
reporter.file_path('math_driven.png'),
[('math_driven', toCartesianCoordAsNp(polar_math_driven, 0, 0))], None)
plotter.draw_contours(
reporter.file_path('math_results.png'),
[('math_drive', toCartesianCoordAsNp(polar_math_drive, 0, 0)),
('math_driven',
np.array(
rotate(
list(
toCartesianCoordAsNp(polar_math_driven, center_distance,
0)), phi[0],
(center_distance, 0))))], [(0, 0), (center_distance, 0)])
logging.info('math rotate complete')
logging.info(f'Center Distance = {center_distance}')
return center_distance, phi, polar_math_drive, polar_math_driven
def optimize_center(cart_input_drive,
cart_input_driven,
debugger,
opt_config,
plotter,
k=1):
debug_suite = ReportingSuite(debugger, plotter,
plt.figure(figsize=(16, 9)))
results = sampling_optimization(
cart_input_drive,
cart_input_driven,
opt_config['sampling_count'],
opt_config['keep_count'],
opt_config['resampling_accuracy'],
opt_config['max_sample_depth'],
debug_suite,
opt_config['torque_weight'],
k=k,
mismatch_penalty=opt_config['mismatch_penalty'])
results.sort(key=lambda total_score, *_: total_score)
best_result = results[0]
logging.info(f'Best result with score {best_result[0]}')
score, polar_drive = best_result
polar_driven, center_distance, phi = compute_dual_gear(polar_drive, k)
drive_contour = toCartesianCoordAsNp(polar_drive, 0, 0)
driven_contour = toCartesianCoordAsNp(polar_driven, center_distance, 0)
driven_contour = np.array(
rotate(driven_contour, phi[0], (center_distance, 0)))
plotter.draw_contours(debugger.file_path('optimize_result.png'),
[('carve_drive', drive_contour),
('carve_driven', driven_contour)],
[(0, 0), (center_distance, 0)])
save_contour(debugger.file_path('optimized_drive.dat'), drive_contour)
save_contour(debugger.file_path('optimized_driven.dat'), driven_contour)
return (0, 0), center_distance, toCartesianCoordAsNp(polar_drive, 0,
0), score
def shape_average(polygon_a: Iterable[float], polygon_b: Iterable[float],
area_a: float, area_b: float) -> np.ndarray:
"""
get the averages of two shapes with respect to polar coordinates from the centroid
:param polygon_a: polygon a in polar coordinates
:param polygon_b: polygon b in polar coordinates, same length as polygon a
:param area_a: area of polygon a, used for normalization
:param area_b: area of polygon b, used for normalization
:return: the average contour, not necessarily uniformly sampled
"""
if hasattr(polygon_a, '__len__') and hasattr(polygon_b, '__len__'):
# noinspection PyTypeChecker
assert len(polygon_a) == len(polygon_b)
# sqrt_a, sqrt_b = [math.sqrt(area_a) for area in (area_a, area_b)]
offset = align(polygon_a, polygon_b)
# return toCartesianCoordAsNp([(ra / sqrt_a + rb / sqrt_b) / 2 for ra, rb in zip(polygon_a, polygon_b)], 0, 0)
return toCartesianCoordAsNp([
(ra + rb) / 2
for ra, rb in zip(polygon_a, polygon_b[offset:] + polygon_b[:offset])
], 0, 0)
def sample_result(drive_contour: np.ndarray, drive_polygon: Polygon,
sample_window: Tuple[float, float, float, float], k: int) \
-> Union[Tuple[float, float, float, np.ndarray], None]:
"""
sample the center of the sample window and get the driven gear
:param drive_contour: the drive gear contour
:param drive_polygon: the driving polygon
:param sample_window: the window in which to take sample (minx, maxx, miny, maxy)
:param k: the drive/driven ratio
:return: center_x, center_y, center_distance, the driven gear | None if not possible
"""
min_x, max_x, min_y, max_y = sample_window
center_x, center_y = (min_x + max_x) / 2, (min_y + max_y) / 2
if not drive_polygon.contains(Point(center_x, center_y)):
return None
polar_coordinates = toExteriorPolarCoord(Point(center_x,
center_y), drive_contour,
drive_contour.shape[0])
driven, center_distance, phi = compute_dual_gear(polar_coordinates, k)
driven_contour = toCartesianCoordAsNp(driven, 0, 0)
return center_x, center_y, center_distance, driven_contour
def sampling_optimization(drive_contour: np.ndarray, driven_contour: np.ndarray, k: int, sampling_count: (int, int),
keep_count: int, resampling_accuracy: int, comparing_accuracy: int, debugger: Reporter,
max_sample_depth: int = 5, max_iteration: int = 1, smoothing: Tuple[int, int] = (0, 0),
visualization: Union[Dict, None] = None, draw_tar_functions: bool = False) \
-> List[Tuple[float, float, float, float, float, np.ndarray, np.ndarray]]:
"""
perform sampling optimization for drive contour and driven contour
:param drive_contour: the driving gear's contour
:param driven_contour: the driven gear's contour
:param k: drive/driven ratio
:param sampling_count: the number of samples in each dimension
:param keep_count: the count of samples kept
:param resampling_accuracy: count of points in the sampling procedure
:param comparing_accuracy: count of samples during comparison
:param debugger: the debugger for storing data
:param max_sample_depth: maximum depth for the sampling optimization to use
:param max_iteration: maximum time for drive/driven to swap and iterate
:param smoothing: smoothing level to be taken by uniform re-sampling
:param visualization: None for no figure, otherwise for visualization configuration
:param draw_tar_functions: True for drawing tar functions in debug windows (affect performance)
:return: final total score, score, center_x, center_y, center_distance, drive contour and driven contour
"""
drive_contour = counterclockwise_orientation(drive_contour)
driven_contour = counterclockwise_orientation(driven_contour)
drive_polygon = Polygon(drive_contour)
driven_polygon = Polygon(driven_contour)
drive_polar = toExteriorPolarCoord(drive_polygon.centroid, drive_contour,
resampling_accuracy)
driven_polar = toExteriorPolarCoord(driven_polygon.centroid,
driven_contour, resampling_accuracy)
drive_smoothing, driven_smoothing = smoothing
drive_contour = getUniformContourSampledShape(drive_contour,
resampling_accuracy,
drive_smoothing > 0)
driven_contour = getUniformContourSampledShape(driven_contour,
resampling_accuracy,
driven_smoothing > 0)
visualize_config = {
'fig_size': (16, 9),
}
subplots = None
if visualization is not None:
visualize_config.update(visualization)
plt.ion()
fig, subplots = plt.subplots(3, 2)
fig.set_size_inches(*visualize_config['fig_size'])
update_polygon_subplots(drive_contour, driven_contour, subplots[0])
debugging_root_directory = debugger.get_root_debug_dir_name()
results = []
# following two variables change during iteration
drive = drive_contour
driven = driven_contour
for iteration_count in range(max_iteration):
debug_directory = os.path.join(debugging_root_directory,
f'iteration_{iteration_count}')
os.makedirs(debug_directory, exist_ok=True)
drive = counterclockwise_orientation(drive)
new_res = sample_drive_gear(
drive, driven_contour, k, sampling_count, keep_count,
comparing_accuracy, max_sample_depth, debug_directory,
subplots[1] if subplots is not None else None)
results += [(None, score, *center, center_distance, drive, driven)
for score, *center, center_distance, driven in new_res]
for index, result in enumerate(results):
total_score, score, *center, center_distance, this_drive, driven = result
if subplots is not None:
update_polygon_subplots(
drive_contour, driven_contour,
subplots[0]) # so that the two subplots can iterate
update_polygon_subplots(this_drive, driven, subplots[1])
subplots[1][0].scatter(center[0], center[1], 3)
subplots[1][0].text(0, 0, str(center))
subplots[1][1].text(0, 0, str(score))
subplots[1][1].scatter(0, 0, 3)
if draw_tar_functions:
tars = [
triangle_area_representation(contour,
comparing_accuracy)
for contour in (this_drive, driven)
]
for subplot, tar in zip(subplots[2], tars):
tar = tar[:, 0]
subplot.clear()
subplot.plot(range(len(tar)), tar, color='blue')
if total_score is None:
total_score = score + shape_difference_rating(
this_drive,
drive_contour,
comparing_accuracy,
distance_function=trivial_distance)
results[index] = (total_score, *result[1:])
score_str = "%.8f" % total_score
plt.savefig(
os.path.join(debug_directory,
f'final_result_{index}_{score_str}.png'))
save_contour(
os.path.join(debug_directory,
f'final_result_{index}_drive.dat'),
this_drive)
save_contour(
os.path.join(debug_directory,
f'final_result_{index}_driven.dat'), driven)
*_, drive, driven = results[-1] # get the last result
drive_contour, driven_contour = driven_contour, drive_contour
drive_polygon, driven_polygon = driven_polygon, drive_polygon
drive_polar, driven_polar = driven_polar, drive_polar
drive, driven = driven, drive
drive_smoothing, driven_smoothing = driven_smoothing, drive_smoothing
# drive_poly = Polygon(drive)
# drive = shape_average(drive_polar, toExteriorPolarCoord(Polygon(drive).centroid, drive, resampling_accuracy),
# drive_polygon.area, drive_poly.area)
drive = phi_average.shape_average(
drive_polar,
toExteriorPolarCoord(
Polygon(drive).centroid, drive, resampling_accuracy))
drive = toCartesianCoordAsNp(drive, 0, 0)
drive = getUniformContourSampledShape(drive, resampling_accuracy,
drive_smoothing > 0)
for subplot in subplots[2]:
subplot.clear()
return results
def generate_std_shapes(type: str, n: int, center_point):
if type not in std_shapes:
print(f"Type Error! No {type} found!")
else:
return toCartesianCoordAsNp(std_shapes[type](n), center_point[0], center_point[1])
def sampling_optimization(drive_contour: np.ndarray, driven_contour: np.ndarray, sampling_count: Tuple[int, int],
keep_count: int, sampling_accuracy: int, iteration_count: int,
debugging_suite: ReportingSuite, torque_weight: float = 0.0, k: int = 1,
mismatch_penalty=0.5) -> List[Tuple[float, Polar_T]]:
logger.info(f'Initiating Sampling Optimization with torque_weight = {torque_weight},'
f' mismatch_penalty = {mismatch_penalty}')
logger.info(f'k={k}')
drive_polygon = Polygon(drive_contour)
driven_polygon = Polygon(driven_contour)
min_x, min_y, max_x, max_y = drive_polygon.bounds
window_pairs = (min_x, max_x, min_y, max_y)
min_x, min_y, max_x, max_y = driven_polygon.bounds
window_pairs = [(window_pairs, (min_x, max_x, min_y, max_y))]
x_sample, y_sample = sampling_count
# start iteration
results = [] # dummy
for iteration in range(iteration_count):
path = debugging_suite.debugger.file_path('iteration_' + str(iteration))
os.makedirs(path, exist_ok=True)
# if k == 1:
window_pairs = list(itertools.chain.from_iterable([
itertools.product(split_window(drive_window, x_sample, y_sample),
split_window(driven_window, x_sample, y_sample))
for drive_window, driven_window in window_pairs
]))
# else:
# # do not allow the driven gear with k to move center
# window_pairs = list(itertools.chain.from_iterable([
# itertools.product(split_window(drive_window, x_sample, y_sample),
# [driven_window])
# for drive_window, driven_window in window_pairs
# ]))
results = sample_in_windows(drive_contour, driven_contour, window_pairs, keep_count,
debugging_suite.sub_suite(os.path.join(path, 'result_')),
sampling_accuracy=sampling_accuracy, torque_weight=torque_weight, k=k,
mismatch_penalty=mismatch_penalty)
window_pairs = [(drive_window, driven_window) for _, drive_window, driven_window, *__ in results]
if debugging_suite.plotter is not None:
for index, final_result in enumerate(results):
score, *_, reconstructed_drive, max_phi, m_penalty = final_result
driven, center_distance, phi = compute_dual_gear(reconstructed_drive, k)
final_drive = toCartesianCoordAsNp(reconstructed_drive, 0, 0)
final_driven = np.array(
psf_rotate(toCartesianCoordAsNp(driven, center_distance, 0), phi[0], (center_distance, 0)))
debugging_suite.plotter.draw_contours(
os.path.join(path, f'final_result_{index}_{"%.6f" % (score,)}.png'),
[('carve_drive', final_drive),
('carve_driven', final_driven)],
[(0, 0), (center_distance, 0)])
save_contour(os.path.join(path, f'final_result_{index}_drive.dat'), final_drive)
save_contour(os.path.join(path, f'final_result_{index}_driven.dat'), final_driven)
d_drive = differentiate_function(pre_process(phi))
save_information(os.path.join(path, f'final_result_{index}.txt'), (0, 0), (center_distance, 0), score,
max_dphi_drive=max(d_drive),
actual_distance=score - torque_weight * max_phi,
mismatch_penalty=m_penalty)
results = results[:keep_count]
results.sort(key=lambda dist, *_: dist)
results = [(score, reconstructed_drive)
for score, drive_window, driven_window, reconstructed_drive, max_phi, m_penalty in results]
return results
def sample_in_windows(drive_contour: np.ndarray, driven_contour: np.ndarray,
window_pairs: List[Tuple[Window_T, Window_T]], keep_count: int,
debugging_suite: ReportingSuite, k: int = 1, center_determine_function=center_of_window,
sampling_accuracy=1024, torque_weight=0.0, mismatch_penalty: float = 0.5) \
-> List[Tuple[float, Window_T, Window_T, Polar_T, float, float]]:
"""
find the best sample windows
:param drive_contour: the drive contour
:param driven_contour: the driven contour
:param window_pairs: pair of windows
:param keep_count: count of the windows to be kept
:param debugging_suite: the debugging suite
:param k: the k of the opposing gear
:param center_determine_function: function to determine from window to center
:param sampling_accuracy: number of samples when converting to polar contour
:param torque_weight: weight of torque term
:param mismatch_penalty: penalty for the extended d_driven not matching start and end (only for k>1)
:return: list of (score, drive_window, driven_window, reconstructed_drive, max_phi, mismatch_penalty)
"""
results = []
path_prefix = debugging_suite.path_prefix # store in a directory
if debugging_suite.figure is not None:
debugging_suite.figure.clear() # clear the figure
plt.figure(debugging_suite.figure.number)
subplots = debugging_suite.figure.subplots(2, 2)
update_polygon_subplots(drive_contour, driven_contour, subplots[0])
else:
subplots = None
for index, (drive_window, driven_window) in enumerate(window_pairs):
center_drive = center_determine_function(drive_window)
center_driven = center_determine_function(driven_window)
if not (Polygon(drive_contour).contains(Point(*center_drive)) and Polygon(driven_contour).contains(
Point(*center_driven))):
# not good windows
continue
# polar, dist, phi = compute_dual_gear(
# toExteriorPolarCoord(Point(*center_driven), driven_contour, sampling_accuracy), 1)
# polar, dist, phi = compute_dual_gear(polar, 1)
# d_phi = differentiate_function(pre_process(phi))
# fig, splts = plt.subplots(2, 2, figsize=(9, 9))
# splts[0][1].plot(*driven_contour.transpose())
# splts[0][1].axis('equal')
# splts[1][0].plot(np.linspace(0, 2 * math.pi, len(phi), endpoint=False), phi)
# splts[1][1].plot(np.linspace(0, 2 * math.pi, len(d_phi), endpoint=False), d_phi, color='red')
# plt.show() #checkpoint passed
distance, d_drive, d_driven, dist_drive, dist_driven = \
contour_distance(drive_contour, center_drive, driven_contour, center_driven, sampling_accuracy, k)
# splts[1][1].plot(np.linspace(0, 2 * math.pi, len(d_driven), endpoint=False), d_driven, color='blue')
# plt.show()
reconstructed_drive = rebuild_polar(0.9, align_and_average(d_drive, d_driven, k=k))
list_reconstructed_drive = list(reconstructed_drive)
max_phi = max(differentiate_function(pre_process(compute_dual_gear(list_reconstructed_drive)[-1])))
final_score = distance + torque_weight * max_phi
m_penalty = None
if k != 1:
m_penalty = mismatch_penalty * abs(d_driven[0] - d_driven[-1])
logger.info(f'{index} gear: mismatching start = {d_driven[0]}, end = {d_driven[-1]}, penalized {m_penalty}')
final_score += m_penalty
logging.info(f'{index} gear: plain score={distance}, max of phi\'={max_phi}')
results.append((final_score, drive_window, driven_window, list_reconstructed_drive, max_phi, m_penalty))
if subplots is not None:
update_polygon_subplots(drive_contour, driven_contour, subplots[0]) # clear sample regions
reconstructed_driven, plt_center_dist, plt_phi = compute_dual_gear(list_reconstructed_drive, k)
reconstructed_drive_contour = toCartesianCoordAsNp(reconstructed_drive, 0, 0)
reconstructed_driven_contour = toCartesianCoordAsNp(reconstructed_driven, 0, 0)
update_polygon_subplots(reconstructed_drive_contour, reconstructed_driven_contour, subplots[1])
min_x, max_x, min_y, max_y = drive_window
sample_region = Rectangle((min_x, min_y), max_x - min_x, max_y - min_y, color='red', fill=False)
subplots[0][0].add_patch(sample_region)
min_x, max_x, min_y, max_y = driven_window
sample_region = Rectangle((min_x, min_y), max_x - min_x, max_y - min_y, color='red', fill=False)
subplots[0][1].add_patch(sample_region)
subplots[0][0].scatter(*center_drive, 5)
subplots[0][1].scatter(*center_driven, 5)
subplots[1][0].scatter(0, 0, 5)
subplots[1][1].scatter(0, 0, 5)
plt.savefig(path_prefix + f'{index}.png')
save_contour(path_prefix + f'{index}_drive.dat', reconstructed_drive_contour)
save_contour(path_prefix + f'{index}_driven.dat', reconstructed_driven_contour)
save_information(path_prefix + f'{index}.txt', center_drive, center_driven, final_score,
max_dphi_drive=max_phi, actual_distance=distance, mismatch_penalty=m_penalty)
# get information about thee phi' functions
original_figure = plt.gcf()
figure, new_subplots = plt.subplots(2, 2, figsize=(16, 16))
new_subplots[0][0].plot(np.linspace(0, 2 * math.pi, len(d_drive), endpoint=False), d_drive)
new_subplots[0][0].axis([0, 2 * math.pi, 0, 2 * math.pi])
new_subplots[0][1].plot(np.linspace(0, 2 * math.pi, len(d_driven), endpoint=False), d_driven)
new_subplots[0][1].axis([0, 2 * math.pi, 0, 2 * math.pi])
new_subplots[1][0].plot(np.linspace(0, 2 * math.pi, len(d_drive), endpoint=False),
align_and_average(d_drive, d_driven, k=k))
new_subplots[1][0].axis([0, 2 * math.pi, 0, 2 * math.pi])
debugging_suite.plotter.draw_contours(
path_prefix + f'drive_contour_{index}.png',
[('carve_drive', drive_contour)],
[center_drive])
debugging_suite.plotter.draw_contours(
path_prefix + f'driven_contour_{index}.png',
[('carve_driven', driven_contour)],
[center_driven])
final_driven = np.array(
psf_rotate(toCartesianCoordAsNp(reconstructed_driven, plt_center_dist, 0), plt_phi[0],
(plt_center_dist, 0)))
debugging_suite.plotter.draw_contours(
path_prefix + f'reconstructed_contour_{index}.png',
[('carve_drive', reconstructed_drive_contour),
('carve_driven', final_driven)],
[(0, 0), (plt_center_dist, 0)])
if k != 1:
# then offset shall be 0
new_subplots[1][1].plot(np.linspace(0, 2 * math.pi, len(d_drive), endpoint=False),
extend_part(d_driven, 0, int(len(d_driven) / k), len(d_drive)))
new_subplots[1][1].axis('equal')
plt.axis('equal')
plt.savefig(path_prefix + f'{index}_functions.png')
plt.close()
plt.figure(original_figure.number)
results.sort(key=lambda dist, *_: dist)
return results[:keep_count]