96 lines
3.3 KiB
Python
96 lines
3.3 KiB
Python
import svgpathtools
|
|
import toppra as ta
|
|
import toppra.constraint as constraint
|
|
import toppra.algorithm as algo
|
|
import numpy as np
|
|
|
|
|
|
# Function to extract and subdivide paths into continuous subpaths
|
|
def extract_svg_paths(svg_file):
|
|
paths, attributes = svgpathtools.svg2paths(svg_file)
|
|
continuous_paths = []
|
|
|
|
for path in paths:
|
|
subpath = []
|
|
last_point = None
|
|
for seg in path:
|
|
if last_point is not None and seg.start != last_point:
|
|
continuous_paths.append(svgpathtools.Path(*subpath))
|
|
subpath = []
|
|
subpath.append(seg)
|
|
last_point = seg.end
|
|
if subpath:
|
|
continuous_paths.append(svgpathtools.Path(*subpath))
|
|
|
|
return continuous_paths
|
|
|
|
|
|
# Function to compute time-reparametrized velocities using toppra based on segment type
|
|
def compute_reparametrized_speeds(path, velocity_limit=.5, acceleration_limit=1):
|
|
sampled_points = []
|
|
ss = []
|
|
current_s = 0.0
|
|
|
|
for k, seg in enumerate(path):
|
|
num_samples = 20 if isinstance(seg, svgpathtools.CubicBezier) else 10
|
|
n = len(path)
|
|
if k < n - 1:
|
|
segment_points = np.array([[seg.point(t).real, seg.point(t).imag] for t in np.linspace(0, 1, num_samples)][:-1])
|
|
sampled_points.append(segment_points)
|
|
ss.extend(list(np.linspace(current_s, current_s + 1, num_samples))[:-1])
|
|
current_s += 1
|
|
else:
|
|
segment_points = np.array([[seg.point(t).real, seg.point(t).imag] for t in np.linspace(0, 1, num_samples)])
|
|
sampled_points.append(segment_points)
|
|
ss.extend(np.linspace(current_s, current_s + 1, num_samples))
|
|
current_s += 1
|
|
|
|
sampled_points = np.vstack(sampled_points)
|
|
ss = np.array(ss) # Ensure correct shape
|
|
velocity_limits = np.array([[-velocity_limit, velocity_limit]] * sampled_points.shape[1])
|
|
acceleration_limits = np.array([[-acceleration_limit, acceleration_limit]] * sampled_points.shape[1])
|
|
|
|
pc = ta.SplineInterpolator(ss, sampled_points)
|
|
constraints = [
|
|
constraint.JointVelocityConstraint(velocity_limits),
|
|
constraint.JointAccelerationConstraint(acceleration_limits)
|
|
]
|
|
|
|
instance = algo.TOPPRA(constraints, pc, solver_wrapper="seidel")
|
|
sd_sq = instance.compute_parameterization(0, 0)[1]
|
|
return list(np.sqrt(sd_sq))
|
|
|
|
|
|
|
|
def path_to_gcode(path, speeds):
|
|
gcode = []
|
|
speed_index = 0
|
|
for seg in path:
|
|
num_samples = 20 if isinstance(seg, svgpathtools.CubicBezier) else 10
|
|
for t in np.linspace(0, 1, num_samples):
|
|
point = seg.point(t)
|
|
speed = speeds[speed_index] if speed_index < len(speeds) else 1
|
|
if speed != 0:
|
|
gcode.append(f"G1 X{point.real:.3f} Y{point.imag:.3f} F{speed:.3f}")
|
|
speed_index += 1
|
|
|
|
return '\n'.join(gcode)
|
|
|
|
|
|
# Main function to process SVG to G-code
|
|
def svg_to_gcode(svg_file):
|
|
paths = extract_svg_paths(svg_file)
|
|
gcode_output = ["G21 ; Set units to mm", "G90 ; Absolute positioning"]
|
|
|
|
for path in paths:
|
|
p_start = path.point(0)
|
|
gcode_output.append(f"G0 X{p_start.real:.3f} Y{p_start.imag:.3f}")
|
|
speeds = compute_reparametrized_speeds(path)
|
|
gcode_output.append(path_to_gcode(path, speeds))
|
|
|
|
return '\n'.join(gcode_output)
|
|
|
|
print(svg_to_gcode("clock.svg"))
|
|
|
|
# Example usage
|
|
# print(svg_to_gcode("drawing.svg"))
|