def gradual_average(drive_model: Model, driven_model: Model, drive_center: Tuple[float, float],
driven_center: Tuple[float, float], count_of_averages: int):
"""
Gradually average two contours
:param drive_model: The drive model
:param driven_model: The driven model
:param drive_center: center of drive
:param driven_center: center of driven
:param count_of_averages: count of average values
:return: None
"""
debugger, opt_config, plotter = init((drive_model, driven_model), 'optimization_config.yaml')
drive_contour, driven_contour = get_inputs(debugger, drive_model, driven_model, plotter)
distance, d_drive, d_driven, dist_drive, dist_driven = \
contour_distance(drive_contour, drive_center, driven_contour, driven_center, 1024)
for average in np.linspace(0, 1, count_of_averages, True):
center_dist = dist_drive * 0.5 + dist_driven * 0.5
reconstructed_drive = rebuild_polar(center_dist, align_and_average(d_drive, d_driven, average))
reconstructed_driven, center_dist, phi = compute_dual_gear(list(reconstructed_drive))
reconstructed_drive_contour = toCartesianCoordAsNp(reconstructed_drive, 0, 0)
reconstructed_driven_contour = toCartesianCoordAsNp(reconstructed_driven, center_dist, 0)
reconstructed_driven_contour = np.array(rotate(reconstructed_driven_contour, phi[0], (center_dist, 0)))
average_str = '%1.8f' % average
plotter.draw_contours(debugger.file_path(average_str + '.png'), [
('math_drive', reconstructed_drive_contour),
('math_driven', reconstructed_driven_contour)
], [(0, 0), (center_dist, 0)])
save_contour(debugger.file_path(average_str + '_drive.dat'), reconstructed_drive_contour)
save_contour(debugger.file_path(average_str + '_driven.dat'), reconstructed_driven_contour)
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 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 rotate_and_cut(drive_polygon: Polygon,
center_distance,
phi,
k=1,
debugger: Reporter = None,
replay_animation: bool = False,
plot_x_range: Tuple[float, float] = (-1.5, 3),
plot_y_range: Tuple[float, float] = (-2.25, 2.25),
save_rate: int = 4,
plotter: Optional[Plotter] = None):
# save_rate: save 1 frame per save_rate frames
from shapely.affinity import translate, rotate
driven_polygon = to_polygon([center_distance] * len(phi))
delta_theta = 2 * pi / len(phi) * k
driven_polygon = translate(driven_polygon, center_distance)
complete_phi = phi + [phi[0]] # so that it rotates back
phi_incremental = [0.0] + [
complete_phi[i] - complete_phi[i - 1]
for i in range(1, len(complete_phi))
]
assert isclose(sum(phi_incremental) % (2 * pi), 0, rel_tol=1e-5)
angle_sum = 0
fig, subplot = plt.subplots(figsize=(7, 7))
subplot.set_title('Dual Shape(Cut)')
subplot.axis('equal')
plt.ion()
for index, angle in enumerate(phi_incremental):
angle_sum = delta_theta * index
_drive_polygon = rotate(drive_polygon,
angle_sum,
use_radians=True,
origin=(0, 0))
driven_polygon = rotate(driven_polygon,
angle,
use_radians=True,
origin=(center_distance, 0))
driven_polygon = driven_polygon.difference(_drive_polygon)
_plot_polygon((_drive_polygon, driven_polygon),
plot_x_range + plot_y_range)
plt.scatter((0, center_distance), (0, 0), s=100, c='b')
if debugger is not None and index % save_rate == 0:
file_path = os.path.join(debugger.get_cutting_debug_dir_name(),
f'before_cut_{index // save_rate}.png')
if plotter is None:
fig.savefig(file_path)
else:
plotter.draw_contours(
file_path,
polygon_to_contour('carve_drive', _drive_polygon) +
polygon_to_contour('carve_driven', driven_polygon),
[(center_distance, 0), (0, 0)])
plt.pause(0.00001)
assert isclose(angle_sum, 2 * pi * k, rel_tol=1e-5)
plt.ioff()
driven_polygon = rotate(driven_polygon,
-complete_phi[-1],
use_radians=True,
origin=(center_distance,
0)) # de-rotate to match phi
if replay_animation:
# replay the animation
plt.ion()
for index, angle in enumerate(phi):
theta = delta_theta * index
_drive_polygon = rotate(drive_polygon, theta, (0, 0), True)
_driven_polygon = rotate(driven_polygon, angle,
(center_distance, 0), True)
_plot_polygon((_drive_polygon, _driven_polygon),
plot_x_range + plot_y_range)
plt.scatter((0, center_distance), (0, 0), s=100, c='b')
if debugger is not None and index % save_rate == 0:
file_path = os.path.join(
debugger.get_cutting_debug_dir_name(),
f'after_cut_{index // save_rate}.png')
if plotter is None:
fig.savefig(file_path)
else:
plotter.draw_contours(
file_path,
polygon_to_contour('carve_drive', _drive_polygon) +
polygon_to_contour('carve_driven', _driven_polygon),
[(center_distance, 0), (0, 0)])
plt.pause(0.001)
plt.ioff()
driven_polygon = translate(driven_polygon, -center_distance)
return driven_polygon, fig, subplot