Klipper-G3圆弧路径算法

gcode命令

• G3: 用于绘制一个逆时针方向的圆弧，G3 X<目标X> Y<目标Y> I<圆心X> J<圆心Y> F<速度> E<流量>
• G2：绘制一个顺时针方向的圆弧

调试数据

• G3 X0 Y-155 I155 Z0.3 E30 F2000

coords 结果

[[-154.9967616230689, -1.001941296463585, 0.3995884773662552],[-154.98704662759278, -2.003840726300321, 0.3991769547325103],[-154.97085541951773, -3.0056564246327775, 0.39876543209876547],[-154.94818867540155, -4.0073465300822875, 0.3983539094650206],[-154.9190473423857, -5.008869186518145, 0.39794238683127575],[-154.88343263815565, -6.01018254480659, 0.39753086419753086],[-154.8413460508899, -7.011244764559488, 0.39711934156378603],[-154.7927893391979, -8.012014015882658, 0.3967078189300412],[-154.73776453204655, -9.012448481123753, 0.3962962962962963],[-154.67627392867547, -10.012506356619632, 0.3958847736625515],[-154.60832009850077, -11.012145854443157, 0.3954732510288066],[-154.53390588100777, -12.011325204149294, 0.39506172839506176],[-154.4530343856325, -13.010002654520557, 0.3946502057613169],[-154.36570899163144, -14.008136475311558, 0.39423868312757204],[-154.27193334794072, -15.00568495899276, 0.39382716049382716],[-154.17171137302321, -16.00260642249323, 0.3934156378600823],[-154.06504725470523, -16.99885920894239, 0.3930041152263375],[-153.95194545000118, -17.994401689410683, 0.3925925925925926],[-153.83241068492748, -18.98919226464904, 0.39218106995884777],[-153.7064479543052, -19.98318936682713, 0.3917695473251029],[-153.574062521551, -20.97635146127032, 0.39135802469135805],[-153.4352599184577, -21.96863704819517, 0.39094650205761317],[-153.29004594496254, -22.960004664443563, 0.39053497942386833],[-153.1384266689054, -23.950412885215275, 0.3901234567901235],[-152.9804084257748, -24.93982032579888, 0.3897119341563786],[-152.81599781844346, -25.928185643301074, 0.3893004115226338],[-152.64520171689225, -26.9154675383742, 0.3888888888888889],[-152.46802725792318, -27.90162475694196, 0.38847736625514406],[-152.28448184486115, -28.88661609192322, 0.3880658436213992],[-152.09457314724463, -29.8704003849539, 0.38765432098765434],[-151.89830910050517, -30.852936528106778, 0.38724279835390946],[-151.69569790563585, -31.83418346560921, 0.3868312757201646],[-151.48674802884852, -32.814100195558666, 0.3864197530864198],[-151.27146820122005, -33.792645771636, 0.3860082304526749],[-151.04986741832764, -34.769779304816474, 0.3855967078189301],[-150.82195493987268, -35.74545996507822, 0.3851851851851852],[-150.58774028929412, -36.719646983108454, 0.38477366255144035],[-150.34723325337023, -37.69229965200696, 0.38436213991769547],[-150.10044388180992, -38.663377328987096, 0.38395061728395063],[-149.8473824868326, -39.632839437074054, 0.3835390946502058],[-149.58805964273748, -40.60064546680041, 0.3831275720164609],[-149.32248618546146, -41.56675497789879, 0.3827160493827161],[-149.0506732121266, -42.53112760099172, 0.3823045267489712],[-148.77263208057636, -43.49372303927851, 0.38189300411522636],[-148.48837440890077, -44.45450107021898, 0.3814814814814815],[-148.19791207495135, -45.4134215472143, 0.38106995884773665],[-147.90125721584448, -46.370444401284445, 0.38065843621399176],[-147.59842222745436, -47.32552964274253, 0.3802469135802469],[-147.28941976389513, -48.2786373628658, 0.3798353909465021],[-146.97426273699185, -49.22972773556326, 0.3794238683127572],[-146.65296431574131, -50.178761019039754, 0.3790123456790124],[-146.3255379257615, -51.12569755745669, 0.3786008230452675],[-145.99199724873068, -52.07049778258903, 0.37818930041152266],[-145.6523562218158, -53.01312221547865, 0.37777777777777777],[-145.30662903708992, -53.95353146808402, 0.37736625514403294],[-144.9548301409394, -54.89168624492608, 0.3769547325102881],[-144.5969742334601, -55.82754734473011, 0.3765432098765432],[-144.23307626784313, -56.76107566206393, 0.3761316872427984],[-143.8631514497502, -57.692232188971836, 0.3757201646090535],[-143.48721523667803, -58.62097801660458, 0.37530864197530867],[-143.10528333731253, -59.54727433684521, 0.3748971193415638],[-142.71737171087244, -60.47108244393071, 0.37448559670781895],[-142.32349656644246, -61.3923637360693, 0.3740740740740741],[-141.9236743622959, -62.31107971705345, 0.3736625514403292],[-141.51792180520698, -63.22719199786846, 0.3732510288065844],[-141.10625584975273, -64.14066229829656, 0.3728395061728395],[-140.68869369760455, -65.0514524485165, 0.3724279835390947],[-140.26525279680942, -65.95952439069845, 0.3720164609053498],[-139.83595084106085, -66.86484018059429, 0.37160493827160496],[-139.40080576895943, -67.7673619891231, 0.37119341563786007],[-138.95983576326344, -68.66705210395195, 0.37078189300411524],[-138.5130592501289, -69.56387293107163, 0.3703703703703704],[-138.06049489833975, -70.45778699636759, 0.3699588477366255],[-137.60216161852767, -71.34875694718578, 0.3695473251028807],[-137.13807856238196, -72.23674555389351, 0.3691358024691358],[-136.66826512184923, -73.12171571143504, 0.36872427983539097],[-136.19274092832316, -74.00363044088209, 0.3683127572016461],[-135.7115258518241, -74.88245289097895, 0.36790123456790125],[-135.22464000016882, -75.75814633968245, 0.36748971193415636],[-134.73210371813042, -76.63067419569626, 0.36707818930041153],[-134.233937586588, -77.49999999999999, 0.3666666666666667],[-133.73016242166682, -78.36608742737262, 0.3662551440329218],[-133.22079927386852, -79.22890028791032, 0.365843621399177],[-132.70586942719132, -80.08840252853874, 0.3654320987654321],[-132.18539439824087, -80.94455823451946, 0.36502057613168726],[-131.6593959353311, -81.79733163095065, 0.36460905349794237],[-131.12789601757538, -82.64668708426213, 0.36419753086419754],[-130.59091685396817, -83.4925891037041, 0.3637860082304527],[-130.04848088245708, -84.33500234283031, 0.3633744855967078],[-129.50061076900516, -85.17389160097493, 0.362962962962963],[-128.9473294066438, -86.00922182472351, 0.3625514403292181],[-128.38865991451627, -86.84095810937762, 0.36213991769547327],[-127.82462563691155, -87.66906570041347, 0.3617283950617284],[-127.25525014228887, -88.49350999493402, 0.36131687242798355],[-126.680557222293, -89.31425654311494, 0.3609053497942387],[-126.10057089075998, -90.13127104964413, 0.36049382716049383],[-125.51531538271375, -90.94451937515475, 0.360082304526749],[-124.92481515335353, -91.75396753765176, 0.3596707818930041],[-124.32909487703179, -92.55958171393186, 0.3592592592592593],[-123.7281794462234, -93.36132824099677, 0.3588477366255144],[-123.12209397048538, -94.15917361745994, 0.35843621399176956],[-122.5108637754077, -94.95308450494645, 0.3580246913580247],[-121.89451440155503, -95.74302772948585, 0.35761316872427984],[-121.27307160339956, -96.5289702828986, 0.357201646090535],[-120.6465613482448, -97.31087932417527, 0.3567901234567901],[-120.01500981514052, -98.08872218084873, 0.3563786008230453],[-119.37844339378886, -98.86246635035943, 0.3559670781893004],[-118.73688868344159, -99.63207950141359, 0.35555555555555557],[-118.09037249178874, -100.39752947533414, 0.3551440329218107],[-117.43892183383828, -101.15878428740443, 0.35473251028806585],[-116.78256393078742, -101.91581212820486, 0.35432098765432096],[-116.12132620888508, -102.66858136494193, 0.35390946502057613],[-115.45523629828591, -103.41706054277003, 0.3534979423868313],[-114.78432203189571, -104.16121838610597, 0.3530864197530864],[-114.10861144420848, -104.90102379993559, 0.3526748971193416],[-113.42813277013488, -105.63644587111332, 0.3522633744855967],[-112.74291444382256, -106.3674538696537, 0.35185185185185186],[-112.0529850974679, -107.09401725001558, 0.351440329218107],[-111.35837356011966, -107.8161056523785, 0.35102880658436214],[-110.65910885647436, -108.53368890391111, 0.3506172839506173],[-109.95522020566341, -109.24673702003217, 0.3502057613168724],[-109.2467370200322, -109.95522020566338, 0.3497942386831276],[-108.53368890391111, -110.65910885647435, 0.3493827160493827],[-107.81610565237851, -111.35837356011965, 0.34897119341563787],[-107.09401725001561, -112.05298509746788, 0.348559670781893],[-106.3674538696537, -112.74291444382254, 0.34814814814814815],[-105.63644587111334, -113.42813277013487, 0.3477366255144033],[-104.90102379993561, -114.10861144420846, 0.34732510288065843],[-104.16121838610597, -114.7843220318957, 0.3469135802469136],[-103.41706054277005, -115.4552362982859, 0.3465020576131687],[-102.66858136494193, -116.12132620888507, 0.3460905349794239],[-101.91581212820486, -116.78256393078742, 0.345679012345679],[-101.15878428740446, -117.43892183383826, 0.34526748971193416],[-100.39752947533415, -118.09037249178874, 0.34485596707818933],[-99.6320795014136, -118.73688868344159, 0.34444444444444444],[-98.86246635035944, -119.37844339378884, 0.3440329218106996],[-98.08872218084873, -120.01500981514052, 0.3436213991769547],[-97.31087932417527, -120.6465613482448, 0.3432098765432099],[-96.52897028289863, -121.27307160339954, 0.342798353909465],[-95.74302772948585, -121.89451440155503, 0.34238683127572017],[-94.95308450494645, -122.5108637754077, 0.34197530864197534],[-94.15917361745997, -123.12209397048537, 0.34156378600823045],[-93.36132824099678, -123.7281794462234, 0.3411522633744856],[-92.55958171393188, -124.32909487703178, 0.34074074074074073],[-91.75396753765177, -124.9248151533535, 0.3403292181069959],[-90.94451937515475, -125.51531538271375, 0.339917695473251],[-90.13127104964416, -126.10057089075997, 0.3395061728395062],[-89.31425654311495, -126.68055722229299, 0.3390946502057613],[-88.49350999493402, -127.25525014228887, 0.33868312757201646],[-87.66906570041351, -127.82462563691153, 0.3382716049382716],[-86.84095810937765, -128.38865991451627, 0.33786008230452674],[-86.00922182472353, -128.94732940664377, 0.3374485596707819],[-85.17389160097495, -129.50061076900514, 0.337037037037037],[-84.33500234283032, -130.04848088245706, 0.3366255144032922],[-83.49258910370412, -130.59091685396817, 0.3362139917695473],[-82.64668708426217, -131.12789601757535, 0.3358024691358025],[-81.79733163095067, -131.6593959353311, 0.3353909465020576],[-80.94455823451946, -132.18539439824087, 0.33497942386831275],[-80.08840252853877, -132.7058694271913, 0.3345679012345679],[-79.22890028791035, -133.2207992738685, 0.33415637860082303],[-78.36608742737262, -133.73016242166682, 0.3337448559670782],[-77.50000000000001, -134.23393758658798, 0.3333333333333333],[-76.63067419569629, -134.7321037181304, 0.3329218106995885],[-75.75814633968245, -135.22464000016882, 0.3325102880658436],[-74.88245289097898, -135.71152585182406, 0.33209876543209876],[-74.00363044088212, -136.19274092832316, 0.33168724279835393],[-73.12171571143506, -136.66826512184923, 0.33127572016460904],[-72.23674555389354, -137.13807856238193, 0.3308641975308642],[-71.34875694718582, -137.60216161852765, 0.3304526748971193],[-70.4577869963676, -138.06049489833975, 0.3300411522633745],[-69.56387293107166, -138.5130592501289, 0.3296296296296296],[-68.66705210395199, -138.9598357632634, 0.3292181069958848],[-67.76736198912312, -139.40080576895943, 0.32880658436213994],[-66.8648401805943, -139.83595084106082, 0.32839506172839505],[-65.95952439069848, -140.26525279680942, 0.32798353909465017],[-65.05145244851651, -140.68869369760455, 0.32757201646090534],[-64.14066229829658, -141.10625584975273, 0.3271604938271605],[-63.22719199786848, -141.51792180520698, 0.3267489711934156],[-62.31107971705346, -141.9236743622959, 0.3263374485596708],[-61.39236373606932, -142.32349656644246, 0.32592592592592595],[-60.47108244393074, -142.71737171087244, 0.32551440329218106],[-59.54727433684522, -143.1052833373125, 0.3251028806584362],[-58.6209780166046, -143.487215236678, 0.32469135802469135],[-57.69223218897187, -143.8631514497502, 0.3242798353909465],[-56.761075662063945, -144.23307626784313, 0.3238683127572016],[-55.82754734473013, -144.59697423346006, 0.3234567901234568],[-54.891686244926106, -144.9548301409394, 0.3230452674897119],[-53.95353146808403, -145.30662903708992, 0.3226337448559671],[-53.01312221547867, -145.65235622181578, 0.3222222222222222],[-52.07049778258906, -145.99199724873068, 0.32181069958847736],[-51.1256975574567, -146.3255379257615, 0.3213991769547325],[-50.178761019039776, -146.65296431574131, 0.32098765432098764],[-49.22972773556329, -146.97426273699185, 0.3205761316872428],[-48.278637362865815, -147.28941976389513, 0.3201646090534979],[-47.325529642742545, -147.59842222745436, 0.3197530864197531],[-46.37044440128447, -147.90125721584445, 0.3193415637860082],[-45.413421547214305, -148.19791207495135, 0.31893004115226337],[-44.454501070218996, -148.48837440890077, 0.31851851851851853],[-43.493723039278535, -148.77263208057633, 0.31810699588477365],[-42.53112760099173, -149.0506732121266, 0.3176954732510288],[-41.56675497789881, -149.32248618546146, 0.3172839506172839],[-40.60064546680044, -149.58805964273745, 0.3168724279835391],[-39.63283943707406, -149.8473824868326, 0.3164609053497942],[-38.66337732898711, -150.10044388180992, 0.3160493827160494],[-37.69229965200699, -150.34723325337023, 0.31563786008230454],[-36.71964698310846, -150.58774028929412, 0.31522633744855966],[-35.74545996507824, -150.82195493987268, 0.3148148148148148],[-34.7697793048165, -151.04986741832764, 0.31440329218106994],[-33.79264577163602, -151.27146820122005, 0.3139917695473251],[-32.81410019555868, -151.4867480288485, 0.3135802469135802],[-31.834183465609236, -151.69569790563583, 0.3131687242798354],[-30.85293652810678, -151.89830910050517, 0.31275720164609055],[-29.870400384953914, -152.09457314724463, 0.31234567901234567],[-28.886616091923244, -152.28448184486115, 0.3119341563786008],[-27.90162475694196, -152.46802725792318, 0.31152263374485595],[-26.915467538374216, -152.64520171689225, 0.3111111111111111],[-25.9281856433011, -152.81599781844346, 0.31069958847736623],[-24.93982032579888, -152.9804084257748, 0.3102880658436214],[-23.950412885215286, -153.1384266689054, 0.30987654320987656],[-22.96000466444359, -153.29004594496254, 0.3094650205761317],[-21.96863704819517, -153.4352599184577, 0.3090534979423868],[-20.97635146127033, -153.574062521551, 0.30864197530864196],[-19.983189366827162, -153.70644795430516, 0.3082304526748971],[-18.98919226464904, -153.83241068492748, 0.30781893004115224],[-17.994401689410697, -153.95194545000118, 0.3074074074074074],[-16.998859208942417, -154.06504725470523, 0.3069958847736625],[-16.002606422493262, -154.17171137302321, 0.3065843621399177],[-15.00568495899277, -154.27193334794072, 0.3061728395061728],[-14.008136475311582, -154.36570899163144, 0.30576131687242797],[-13.01000265452059, -154.45303438563246, 0.30534979423868314],[-12.011325204149308, -154.53390588100777, 0.30493827160493825],[-11.012145854443178, -154.60832009850074, 0.3045267489711934],[-10.012506356619669, -154.67627392867547, 0.30411522633744853],[-9.012448481123764, -154.73776453204655, 0.3037037037037037],[-8.012014015882679, -154.7927893391979, 0.3032921810699588],[-7.011244764559521, -154.8413460508899, 0.302880658436214],[-6.010182544806601, -154.88343263815565, 0.30246913580246915],[-5.008869186518168, -154.9190473423857, 0.30205761316872426],[-4.00734653008232, -154.94818867540155, 0.3016460905349794],[-3.0056564246327877, -154.97085541951773, 0.30123456790123454],[-2.0038407263003424, -154.98704662759278, 0.3008230452674897],[-1.0019412964636176, -154.9967616230689, 0.3004115226337448],[0.0, -155.0, 0.3]]

• 长度243，243个坐标点
• 逆时针的四分之一圆弧

代码解析

def cmd_G2(self, gcmd):self._cmd_inner(gcmd, True)def cmd_G3(self, gcmd):self._cmd_inner(gcmd, False)def _cmd_inner(self, gcmd, clockwise):gcodestatus = self.gcode_move.get_status()# 只支持绝对坐标模式if not gcodestatus['absolute_coordinates']:raise gcmd.error("G2/G3 does not support relative move mode")currentPos = gcodestatus['gcode_position']# Parse parameters# 参数解析asX = gcmd.get_float("X", currentPos[0])asY = gcmd.get_float("Y", currentPos[1])asZ = gcmd.get_float("Z", currentPos[2])if gcmd.get_float("R", None) is not None:raise gcmd.error("G2/G3 does not support R moves")asI = gcmd.get_float("I", 0.)asJ = gcmd.get_float("J", 0.)if not asI and not asJ:raise gcmd.error("G2/G3 neither I nor J given")asE = gcmd.get_float("E", None)asF = gcmd.get_float("F", None)# Build list of linear coordinates to move to# 构建圆弧线性移动的坐标coords = self.planArc(currentPos, [asX, asY, asZ], [asI, asJ],clockwise)e_per_move = e_base = 0.if asE is not None:if gcodestatus['absolute_extrude']:e_base = currentPos[3]e_per_move = (asE - e_base) / len(coords)# Convert coords into G1 commands# 遍历列表坐标，调用G1移动命令for coord in coords:g1_params = {'X': coord[0], 'Y': coord[1], 'Z': coord[2]}if e_per_move:g1_params['E'] = e_base + e_per_moveif gcodestatus['absolute_extrude']:e_base += e_per_moveif asF is not None:g1_params['F'] = asFg1_gcmd = self.gcode.create_gcode_command("G1", "G1", g1_params)self.gcode_move.cmd_G1(g1_gcmd)

• 获取G2 或G3 命令参数值
• 构建圆弧线性移动坐标列表
• 遍历坐标列表，调用G1移动命令

路径生成算法

def planArc(self, currentPos, targetPos, offset, clockwise):# todo: sometimes produces full circlesX_AXIS = 0Y_AXIS = 1Z_AXIS = 2# Radius vector from center to current locationr_P = -offset[0]r_Q = -offset[1]# Determine angular travelcenter_P = currentPos[X_AXIS] - r_Pcenter_Q = currentPos[Y_AXIS] - r_Qrt_X = targetPos[X_AXIS] - center_Prt_Y = targetPos[Y_AXIS] - center_Qangular_travel = math.atan2(r_P * rt_Y - r_Q * rt_X,r_P * rt_X + r_Q * rt_Y)if angular_travel < 0.:angular_travel += 2. * math.piif clockwise:angular_travel -= 2. * math.piif (angular_travel == 0.and currentPos[X_AXIS] == targetPos[X_AXIS]and currentPos[Y_AXIS] == targetPos[Y_AXIS]):# Make a circle if the angular rotation is 0 and the# target is current positionangular_travel = 2. * math.pi# Determine number of segmentslinear_travel = targetPos[Z_AXIS] - currentPos[Z_AXIS]radius = math.hypot(r_P, r_Q)flat_mm = radius * angular_travelif linear_travel:mm_of_travel = math.hypot(flat_mm, linear_travel)else:mm_of_travel = math.fabs(flat_mm)segments = max(1., math.floor(mm_of_travel / self.mm_per_arc_segment))# Generate coordinatestheta_per_segment = angular_travel / segmentslinear_per_segment = linear_travel / segmentscoords = []for i in range(1, int(segments)):dist_Z = i * linear_per_segmentcos_Ti = math.cos(i * theta_per_segment)sin_Ti = math.sin(i * theta_per_segment)r_P = -offset[0] * cos_Ti + offset[1] * sin_Tir_Q = -offset[0] * sin_Ti - offset[1] * cos_Tic = [center_P + r_P, center_Q + r_Q, currentPos[Z_AXIS] + dist_Z]coords.append(c)coords.append(targetPos)return coords

1. 参数说明：
   • currentPos: 当前的位置，是一个三维坐标 [X, Y, Z]。
   • targetPos: 目标位置，同样是一个三维坐标 [X, Y, Z]。
   • offset: 当前点到圆心的偏移量 [X_offset, Y_offset]，用于计算圆心位置。
   • clockwise: 表示弧线是否为顺时针（True）或逆时针（False）。
2. 计算角度变化：
   • r_P 和 r_Q: 分别为当前点到圆心的水平和垂直向量。用来计算当前位置与目标位置之间的角度。
   • center_P 和 center_Q: 圆心的坐标。通过将当前点的坐标减去偏移量计算得到。
   • angular_travel: 从当前点到目标点的角度（弧度）。利用两点的向量叉乘和点积，结合 atan2 函数计算出角度。如果计算出来的角度小于零，则加上 2π 使其成为正值。
3. 调整角度方向：
   • 如果是顺时针运动，需要将 angular_travel 减去 2π 进行调整。逆时针则不需要做调整。
   • 如果 angular_travel 等于0，并且当前位置等于目标位置，说明这个弧线是一个完整的圆，因此将角度设为 2π。
4. 分段计算：
   • linear_travel: Z轴上的位移量，计算当前Z坐标和目标Z坐标的差。
   • radius: 当前点到圆心的半径。
   • flat_mm: 沿XY平面的弧线距离，使用 radius * angular_travel 计算。
   • mm_of_travel: 总的路径长度，如果存在Z轴位移，就计算沿XY和Z轴的斜线长度；否则仅计算XY平面的路径长度。
   • segments: 将整个弧线分割成的线段数。通常将路径长度按预设的 mm_per_arc_segment 来分割，以确保路径平滑。
5. 生成坐标点：
   • theta_per_segment: 每段线段的角度。
   • linear_per_segment: 每段线段沿Z轴的位移量。
   • 循环遍历每一段，计算新的坐标，并将这些分段坐标存储在 coords 数组中。最后，目标位置 targetPos 被添加到 coords 中。
6. 返回值：
   • 函数返回的是一系列沿着弧线的坐标点列表 coords，每个点都是 [X, Y, Z] 的三维坐标。3D打印机将按照这些坐标依次移动，从而在空间中绘制出弧形路径。
7. 此函数的用途是生成弧形插值路径，用于控制3D打印机或CNC机床沿着圆弧形轨迹运动，确保在弧线段上以预定的分段步长平稳地移动，避免直线插值导致的路径不平滑问题。