Skip to content

Commit c9c4bfc

Browse files
feecatfeecat
committed
Update test script and boundary image
1 parent 9815792 commit c9c4bfc

File tree

2 files changed

+89
-95
lines changed

2 files changed

+89
-95
lines changed

‎doc/img/Compare_Boundary_3.png‎

85 KB
Loading

‎python/Test_Stress_WithRuckig.py‎

Lines changed: 89 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,12 @@
44
import time
55
import math
66

7+
REPORT_RUCKIG_ISSUES = False
8+
REPORT_S7RTT_ISSUES = True
9+
REPORT_RUCKIG_SLOWS = False
10+
REPORT_S7RTT_SLOWS = False
711

8-
SIM_TOLERANCE = 1e-3
9-
10-
REPORT_RUCKIG_ISSUES = False
12+
SIM_TOLERANCE = 1e-3
1113

1214
try:
1315
from S7RTT import S7RTT, MotionState
@@ -23,21 +25,17 @@
2325
print("Error: Ruckig not install. try 'pip install ruckig'")
2426
sys.exit(1)
2527

26-
def simulate_s7_end_state(start_s: MotionState, nodes):
28+
def simulate_s7_end_state(start_s, nodes):
2729
p, v, a = start_s.p, start_s.v, start_s.a
28-
2930
for node in nodes:
3031
dt = node.dt
3132
j = node.j
3233
if dt <= 0: continue
33-
3434
dt2 = dt * dt
3535
dt3 = dt2 * dt
36-
3736
p += v * dt + 0.5 * a * dt2 + (1.0/6.0) * j * dt3
3837
v += a * dt + 0.5 * j * dt2
3938
a += j * dt
40-
4139
return p, v, a
4240

4341
def run_stress_test_with_sim(num_tests=10000):
@@ -49,14 +47,16 @@ def run_stress_test_with_sim(num_tests=10000):
4947

5048
cnt_total_run = 0
5149

52-
cnt_s7_fail_plan = 0
53-
cnt_s7_fail_sim = 0
50+
cnt_s7_fail_plan = 0
51+
cnt_s7_fail_sim = 0
52+
cnt_rk_fail_plan = 0
53+
cnt_rk_fail_sim = 0
5454

55-
cnt_rk_fail_plan = 0
56-
cnt_rk_fail_sim = 0
55+
cnt_s7_faster = 0
56+
cnt_rk_faster = 0
57+
cnt_draw = 0
5758

58-
cnt_s7_faster = 0
59-
cnt_mismatches = 0
59+
cnt_printed_issues = 0
6060

6161
GLOBAL_J_LIMIT = 1000000.0
6262
GLOBAL_A_LIMIT = 100000.0
@@ -66,167 +66,161 @@ def run_stress_test_with_sim(num_tests=10000):
6666
cnt_total_run += 1
6767

6868
j_max = random.uniform(0.1, GLOBAL_J_LIMIT)
69-
7069
a_upper_bound = min(GLOBAL_A_LIMIT, j_max * 10.0)
7170
a_max = random.uniform(0.1, a_upper_bound)
72-
7371
v_upper_bound = min(GLOBAL_V_LIMIT, a_max * 10.0)
7472
v_max = random.uniform(0.1, v_upper_bound)
7573

7674
p_limit = 5000.0
7775
p_start = random.uniform(-p_limit, p_limit)
7876
p_target = random.uniform(-p_limit, p_limit)
79-
8077
v_start = random.uniform(-v_max, v_max)
8178
v_target = random.uniform(-v_max, v_max)
82-
8379
a_start = random.uniform(-a_max, a_max)
8480
a_target = 0.0
8581

8682
s7_ok = False
8783
s7_dur = 0.0
88-
s7_error_info = ""
89-
s7_sim_end_p = 0.0
84+
s7_error_reason = ""
85+
s7_end_p = 0.0
9086

9187
try:
9288
start_obj = MotionState(0.0, p_start, v_start, a_start, 0.0)
9389
traj = planner_s7.plan(start_obj, p_target, v_target, v_max, a_max, j_max)
9490

9591
if traj and len(traj) > 0:
9692
s7_dur = sum(node.dt for node in traj)
97-
s7_sim_end_p, _, _ = simulate_s7_end_state(start_obj, traj)
93+
s7_end_p, _, _ = simulate_s7_end_state(start_obj, traj)
9894

99-
if abs(s7_sim_end_p - p_target) > SIM_TOLERANCE:
95+
if abs(s7_end_p - p_target) > SIM_TOLERANCE:
10096
s7_ok = False
10197
cnt_s7_fail_sim += 1
102-
s7_error_info = f"Sim Accuracy Failed. Err={abs(s7_sim_end_p - p_target):.4f}"
98+
s7_error_reason = f"Accuracy Fail (Diff={abs(s7_end_p - p_target):.4f})"
10399
else:
104100
s7_ok = True
105101
else:
106-
if abs(p_start - p_target) < 1e-3 and \
107-
abs(v_start - v_target) < 1e-3:
102+
if abs(p_start - p_target) < 1e-3 and abs(v_start - v_target) < 1e-3:
108103
s7_dur = 0.0
109104
s7_ok = True
110105
else:
111106
s7_ok = False
112107
cnt_s7_fail_plan += 1
113-
s7_error_info = "Result is empty list []"
108+
s7_error_reason = "Empty Trajectory"
114109
except Exception:
115110
s7_ok = False
116111
cnt_s7_fail_plan += 1
117-
s7_error_info = traceback.format_exc()
112+
s7_error_reason = traceback.format_exc()
118113

119114
rk_ok = False
120115
rk_dur = 0.0
121-
rk_sim_end_p = 0.0
116+
rk_error_reason = ""
117+
rk_end_p = 0.0
122118

123119
try:
124120
otg = Ruckig(1)
125121
inp = InputParameter(1)
126-
inp.current_position = [p_start]; inp.current_velocity = [v_start]; inp.current_acceleration = [a_start]
127-
inp.target_position = [p_target]; inp.target_velocity = [v_target]; inp.target_acceleration = [a_target]
128-
inp.max_velocity = [v_max]; inp.max_acceleration = [a_max]; inp.max_jerk = [j_max]
122+
inp.current_position = [p_start]
123+
inp.current_velocity = [v_start]
124+
inp.current_acceleration = [a_start]
125+
inp.target_position = [p_target]
126+
inp.target_velocity = [v_target]
127+
inp.target_acceleration = [a_target]
128+
inp.max_velocity = [v_max]
129+
inp.max_acceleration = [a_max]
130+
inp.max_jerk = [j_max]
129131

130132
res_traj = Trajectory(1)
131133
result = otg.calculate(inp, res_traj)
132134

133135
if result.value >= 0:
134136
rk_dur = res_traj.duration
135137
final_state = res_traj.at_time(rk_dur)
136-
rk_sim_end_p = final_state[0][0]
138+
rk_end_p = final_state[0][0]
137139

138-
if abs(rk_sim_end_p - p_target) > SIM_TOLERANCE:
140+
if abs(rk_end_p - p_target) > SIM_TOLERANCE:
139141
rk_ok = False
140-
cnt_rk_fail_sim += 1
142+
cnt_rk_fail_sim += 1
143+
rk_error_reason = f"Accuracy Fail (Diff={abs(rk_end_p - p_target):.4f})"
141144
else:
142145
rk_ok = True
143146
else:
144147
rk_ok = False
145148
cnt_rk_fail_plan += 1
146-
except:
149+
rk_error_reason = f"Plan Fail Code: {result.value}"
150+
except Exception:
147151
rk_ok = False
148152
cnt_rk_fail_plan += 1
153+
rk_error_reason = traceback.format_exc()
149154

150-
should_print = False
151-
reason = ""
155+
winner_label = "DRAW"
156+
if s7_ok and rk_ok:
157+
diff = s7_dur - rk_dur
158+
if diff < -1e-6:
159+
cnt_s7_faster += 1
160+
winner_label = "S7RTT"
161+
elif diff > 1e-6:
162+
cnt_rk_faster += 1
163+
winner_label = "RUCKIG"
164+
else:
165+
cnt_draw += 1
152166

153-
if not s7_ok and "Sim Accuracy" in s7_error_info:
154-
should_print = True
155-
reason = f"S7 Accuracy Fail (Err={abs(s7_sim_end_p - p_target):.3f})"
156-
if not rk_ok:
157-
reason += " [Ruckig also failed]"
167+
should_print = False
168+
report_reason = ""
158169

170+
if not s7_ok:
171+
if REPORT_S7RTT_ISSUES:
172+
should_print = True
173+
report_reason = f"S7RTT Issue: {s7_error_reason}"
159174
elif not rk_ok:
160-
if s7_ok and REPORT_RUCKIG_ISSUES:
175+
if REPORT_RUCKIG_ISSUES:
161176
should_print = True
162-
reason = "S7 Success, RK Failed/Inaccurate"
163-
164-
elif not s7_ok:
165-
should_print = True
166-
reason = "S7 Plan Fail"
167-
177+
report_reason = f"Ruckig Issue: {rk_error_reason}"
168178
else:
169-
if s7_dur < rk_dur - 1e-6:
170-
cnt_s7_faster += 1
171-
else:
172-
diff = abs(s7_dur - rk_dur)
173-
tol = 0.002 + 0.005 * max(s7_dur, rk_dur)
174-
175-
if diff > tol:
176-
should_print = True
177-
reason = f"Duration Mismatch (S7 Slower by {diff:.3f}s)"
179+
if winner_label == "S7RTT" and REPORT_RUCKIG_SLOWS:
180+
should_print = True
181+
report_reason = f"Ruckig Slower (+{s7_dur - rk_dur:.4f}s)"
182+
elif winner_label == "RUCKIG" and REPORT_S7RTT_SLOWS:
183+
should_print = True
184+
report_reason = f"S7RTT Slower (+{rk_dur - s7_dur:.4f}s)"
178185

179186
if should_print:
180-
cnt_mismatches += 1
187+
cnt_printed_issues += 1
181188
print("\n" + "="*80)
182-
print(f"ISSUE FOUND at Test #{i}")
189+
print(f"REPORT at Test #{i} | Reason: {report_reason}")
183190
print("="*80)
184-
print(f"Reason: {reason}")
185-
186-
if not s7_ok and "Sim Accuracy" not in s7_error_info:
187-
if len(s7_error_info) > 5:
188-
print("-" * 40)
189-
print("S7RTT TRACEBACK:")
190-
print(s7_error_info.strip())
191-
print("-" * 40)
192-
193-
print("\nPARAMETERS:")
194-
print(f" v_max = {v_max:.3f}, a_max={a_max:.3f}, j_max={j_max:.3f}")
195-
print(f" start = (p={p_start:.3f}, v={v_start:.3f}, a={a_start:.3f})")
196-
print(f" target = (p={p_target:.3f}, v={v_target:.3f}, a=0.000)")
197-
191+
print(f"PARAMETERS:")
192+
print(f" v_max={v_max:.3f}, a_max={a_max:.3f}, j_max={j_max:.3f}")
193+
print(f" start=(p={p_start:.3f}, v={v_start:.3f}, a={a_start:.3f})")
194+
print(f" target=(p={p_target:.3f}, v={v_target:.3f}, a={a_target:.3f})")
198195
print("-" * 40)
199-
print("RESULTS (Verified):")
196+
print("RESULTS:")
197+
198+
s7_str = f"OK ({s7_dur:.4f}s)" if s7_ok else "FAIL"
199+
rk_str = f"OK ({rk_dur:.4f}s)" if rk_ok else "FAIL"
200200

201-
s7_res_str = f"{s7_dur:.3f}s" if s7_ok else f"FAIL"
202-
rk_res_str = f"{rk_dur:.3f}s" if rk_ok else f"FAIL"
201+
print(f" S7RTT : {s7_str}")
202+
if not s7_ok: print(f" Info: {s7_error_reason}")
203203

204-
if s7_dur > 0: s7_res_str += f" (EndErr={s7_sim_end_p-p_target:.1e})"
205-
if rk_dur > 0: rk_res_str += f" (EndErr={rk_sim_end_p-p_target:.1e})"
204+
print(f" Ruckig : {rk_str}")
205+
if not rk_ok: print(f" Info: {rk_error_reason}")
206206

207-
print(f" S7RTT : {s7_res_str}")
208-
print(f" Ruckig : {rk_res_str}")
209-
print("="*80 + "\n")
207+
print("="*80)
210208

211-
if i % 500 == 0:
212-
print(f"Processing... {i}/{num_tests} cases. (Mismatches: {cnt_mismatches})")
209+
if i % 1000 == 0:
210+
print(f"Progress: {i}/{num_tests} done.")
213211

214212
print("\n" + "#" * 80)
215-
print("FINAL STATISTICS (With Simulation Verification)")
213+
print("FINAL STATISTICS")
216214
print("#" * 80)
217-
print(f"Total Tests : {cnt_total_run}")
215+
print(f"Total Tests: {cnt_total_run}")
218216
print("-" * 40)
219-
print(f"S7RTT Failures:")
220-
print(f" - Plan Crash/Empty: {cnt_s7_fail_plan}")
221-
print(f" - Accuracy check : {cnt_s7_fail_sim}")
217+
print(f"{'CATEGORY':<20} | {'S7RTT':<10} | {'RUCKIG':<10}")
222218
print("-" * 40)
223-
print(f"Ruckig Failures:")
224-
print(f" - Plan Error : {cnt_rk_fail_plan}")
225-
print(f" - Accuracy check : {cnt_rk_fail_sim}")
219+
print(f"{'Plan Failures':<20} | {cnt_s7_fail_plan:<10} | {cnt_rk_fail_plan:<10}")
220+
print(f"{'Sim Acc Failures':<20} | {cnt_s7_fail_sim:<10} | {cnt_rk_fail_sim:<10}")
226221
print("-" * 40)
227-
print(f"Comparisons:")
228-
print(f" - S7 Faster : {cnt_s7_faster} (Optimizations)")
229-
print(f" - Mismatches : {cnt_mismatches} (S7 Slower/Failed)")
222+
print(f"{'Faster Count':<20} | {cnt_s7_faster:<10} | {cnt_rk_faster:<10}")
223+
print(f"{'Draws':<20} | {cnt_draw:<10} | {cnt_draw:<10}")
230224
print("#" * 80)
231225

232226
if __name__ == "__main__":

0 commit comments

Comments
 (0)