Skip to content

Commit 9c4618f

Browse files
feecatfeecat
committed
Refactor and optimize Brent's method solver
1 parent eac1171 commit 9c4618f

1 file changed

Lines changed: 119 additions & 60 deletions

File tree

‎python/S7RTT.py‎

Lines changed: 119 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
# ==============================================================================
22
# File Name: S7RTT.py
33
# Author: feecat
4-
# Version: V1.2
4+
# Version: V1.3
55
# Description: Simple 7seg Real-Time Trajectory Generator
66
# Website: https://github.com/feecat/S7RTT
77
# License: Apache License Version 2.0
@@ -24,79 +24,138 @@
2424

2525
class Solver:
2626
"""
27-
A numerical solver utility using Brent's Method.
28-
Primarily used to find the exact peak velocity required to align the
29-
trajectory's total distance with the target position.
27+
Optimized Numerical Solver using Brent's Method.
3028
"""
3129

32-
EPS_TOL = 1e-8 # Tolerance for function value convergence
33-
EPS_CONVERGE = 1e-12 # Tolerance for interval convergence
34-
MAX_ITER = 50 # Maximum iterations to prevent infinite loops
30+
# Use system machine epsilon (typically around 2.22e-16) for better accuracy
31+
EPS = 1e-16
32+
TOL = 1e-12
33+
MAX_ITER = 100
3534

3635
@staticmethod
37-
def solve_monotonic_brent(func, low, high):
36+
def solve_brent(func, low, high):
3837
"""
39-
Finds the root of a monotonic function 'func' within the interval [low, high].
40-
"""
41-
f_low = func(low)
42-
f_high = func(high)
38+
Finds the root of 'func' in [low, high] using Brent's method.
4339
44-
# If the root is bracketed, return the side closer to zero
45-
if f_low * f_high > 0:
46-
if abs(f_low) < abs(f_high): return low
47-
else: return high
48-
49-
a = low; b = high
50-
fa = f_low; fb = f_high
40+
Returns:
41+
The root x (or the closest approximation if not converged).
42+
"""
43+
a = low
44+
b = high
45+
fa = func(a)
46+
fb = func(b)
47+
48+
# Check if the root is bracketed.
49+
# If fa and fb have the same sign, we cannot guarantee a root exists.
50+
# In this case, return the endpoint closer to zero.
51+
if (fa > 0 and fb > 0) or (fa < 0 and fb < 0):
52+
return a if abs(fa) < abs(fb) else b
53+
54+
if fa == 0: return a
55+
if fb == 0: return b
56+
57+
# Initialize variables
58+
# c is the opposite end of the interval from b
59+
c = a
60+
fc = fa
61+
d = e = b - a
5162

52-
root_est = b
63+
# mflag indicates if bisection is forced
64+
mflag = True
5365

66+
tol_const = Solver.TOL
67+
mach_eps = Solver.EPS
68+
5469
for _ in range(Solver.MAX_ITER):
55-
# Ensure 'b' is the better estimate (closer to 0)
56-
if abs(fa) < abs(fb):
57-
a, b = b, a
58-
fa, fb = fb, fa
59-
60-
# Check for convergence
61-
if abs(fb - fa) < Solver.EPS_CONVERGE:
62-
root_est = b
63-
break
64-
else:
65-
# Try Inverse Quadratic Interpolation
66-
if abs(fa - fb) > 1e-14:
67-
root_est = b - fb * (b - a) / (fb - fa)
68-
else:
69-
root_est = b
70-
71-
mid_val = 0.5 * (a + b)
70+
# Ensure |f(b)| <= |f(c)| so that b is always the best guess
71+
if abs(fc) < abs(fb):
72+
a, b, c = b, c, b
73+
fa, fb, fc = fb, fc, fb
7274

73-
# Conditions to fall back to Bisection method
74-
cond1 = (root_est < mid_val and b < mid_val)
75-
cond2 = (root_est > mid_val and b > mid_val)
75+
# Calculate tolerance
76+
# tol1 combines relative machine precision and absolute tolerance
77+
tol1 = 2.0 * mach_eps * abs(b) + 0.5 * tol_const
78+
xm = 0.5 * (c - b)
7679

77-
if cond1 or cond2:
78-
root_est = mid_val
80+
# Convergence test: interval is small enough or exact root found
81+
if abs(xm) <= tol1 or fb == 0:
82+
return b
7983

80-
# Ensure estimate stays within bounds
81-
min_val = a if a < b else b
82-
max_val = a if a > b else b
83-
84-
if root_est < min_val or root_est > max_val:
85-
root_est = mid_val
84+
# Attempt interpolation if:
85+
# 1. Previous step size |e| was larger than tolerance
86+
# 2. Function value at 'a' is significantly different from 'b'
87+
if abs(e) >= tol1 and abs(fa) > abs(fb):
88+
s = fb / fa
8689

87-
f_root = func(root_est)
88-
89-
# Check if result is within tolerance
90-
if abs(f_root) < Solver.EPS_TOL or abs(b - a) < Solver.EPS_TOL:
91-
return root_est
90+
if a == c:
91+
# Linear interpolation (Secant Method)
92+
p = 2.0 * xm * s
93+
q = 1.0 - s
94+
else:
95+
# Inverse Quadratic Interpolation (IQI)
96+
q = fa / fc
97+
r = fb / fc
98+
p = s * (2.0 * xm * q * (q - r) - (b - a) * (r - 1.0))
99+
q = (q - 1.0) * (r - 1.0) * (s - 1.0)
100+
101+
# Adjust signs of p and q ensuring p > 0
102+
if p > 0:
103+
q = -q
104+
p = abs(p)
105+
106+
# Validate if interpolation result is acceptable:
107+
# 1. Must fall strictly within (b, b + 3*xm)
108+
# 2. Must converge faster than bisection (checked against min2)
109+
min1 = 3.0 * xm * q - abs(tol1 * q)
110+
min2 = abs(e * q)
111+
112+
if 2.0 * p < min1:
113+
# Check convergence speed relative to previous step
114+
if mflag:
115+
# Previous step was bisection; require significant reduction
116+
if 2.0 * p < min2:
117+
e = d
118+
d = p / q
119+
mflag = False
120+
else:
121+
d = xm; e = d; mflag = True
122+
else:
123+
# Previous step was interpolation; check against d form step before last
124+
if 2.0 * p < abs(d * q):
125+
e = d
126+
d = p / q
127+
mflag = False
128+
else:
129+
d = xm; e = d; mflag = True
130+
else:
131+
# Interpolation failed (out of bounds), fallback to bisection
132+
d = xm; e = d; mflag = True
133+
else:
134+
# Step too small or function too flat -> force bisection
135+
d = xm; e = d; mflag = True
136+
137+
# Apply the computed step
138+
a = b; fa = fb # Store previous b as a (for the next bracket)
92139

93-
# Update the brackets based on the sign
94-
if fa * f_root < 0:
95-
b = root_est; fb = f_root
140+
if abs(d) > tol1:
141+
b += d
96142
else:
97-
a = root_est; fa = f_root
143+
# If step is too small, force a tiny step to avoid stagnation
144+
b += math.copysign(tol1, xm)
98145

99-
return root_est
146+
fb = func(b)
147+
148+
# Maintain bracket [b, c] such that f(b) and f(c) have opposite signs.
149+
# If f(b) and f(c) share the same sign, the root is no longer between them.
150+
# Reset c to a (the previous b).
151+
if (fb > 0 and fc > 0) or (fb < 0 and fc < 0):
152+
c = a
153+
fc = fa
154+
d = e = b - a # Reset step size
155+
mflag = True
156+
157+
# If max iterations reached, return the best guess b
158+
return b
100159

101160

102161
class MotionState:
@@ -358,14 +417,14 @@ def get_error(v_p):
358417

359418
if s_low >= s_high: s_low = s_high - S7RTT.EPS_VAL
360419

361-
best_v = Solver.solve_monotonic_brent(get_error, s_low, s_high)
420+
best_v = Solver.solve_brent(get_error, s_low, s_high)
362421

363422
# Strategy C: Fallback Mechanism
364423
hit_low = abs(best_v - s_low) < S7RTT.EPS_VAL
365424
hit_high = abs(best_v - s_high) < S7RTT.EPS_VAL
366425

367426
if hit_low or hit_high:
368-
best_v = Solver.solve_monotonic_brent(get_error, -limit_safe, limit_safe)
427+
best_v = Solver.solve_brent(get_error, -limit_safe, limit_safe)
369428

370429
# Post-Process: Micro-noise filtering
371430
# If result implies v_inertial, use the cached shapes to save calculation

0 commit comments

Comments
 (0)