# Test_100.py

import random

from S7RTT import S7RTT, MotionState

def run_stress_test():

print("============================================================")

print(" S7RTT STRESS TEST (100 Runs) ")

print("============================================================")

planner = S7RTT()

# Constraints

v_lim = 10.0

a_lim = 10.0

j_lim = 10.0

fail_count = 0

pass_count = 0

# random.seed(42)

for i in range(1, 101):

# 1. Generate random initial state

# Note: MotionState now requires dt. For start state, dt=0.

p0 = random.uniform(-500, 500)

v0 = random.uniform(-2.0 * v_lim, 2.0 * v_lim)

a0 = random.uniform(-2.0 * a_lim, 2.0 * a_lim)

# Use keyword arguments for clarity with the new structure

start_state = MotionState(dt=0.0, p=p0, v=v0, a=a0, j=0.0)

# 2. Generate random target state

pt = random.uniform(-500, 500)

vt = random.uniform(-v_lim, v_lim)

# 3. Run planning

try:

# 'trajectory' is now a list of MotionState objects

trajectory = planner.plan(start_state, pt, vt, v_lim, a_lim, j_lim)

if not trajectory:

# If for some reason the plan is empty (e.g. invalid limits), count as fail

raise ValueError("Planner returned empty trajectory.")

# 4. Verify results

# Calculate total duration from the MotionState nodes

total_duration = sum(node.dt for node in trajectory)

# Use the propagator to find the state at the EXACT end of the trajectory

final_state = planner.at_time(trajectory, total_duration)

# Check errors

p_err = abs(final_state.p - pt)

v_err = abs(final_state.v - vt)

# Target acceleration and jerk should be 0 at the end of a standard S-Curve

# (unless interrupted, but here we expect full completion)

a_err = abs(final_state.a)

# Allowed tolerance

tol = 1e-3

is_success = (p_err < tol) and (v_err < tol) and (a_err < tol)

if is_success:

pass_count += 1

print(f"[RUN {i:04d}] PASS | Time:{total_duration:.2f}s")

else:

fail_count += 1

print(f"[RUN {i:04d}] FAIL !!!")

print(f" Start : P={p0:.2f}, V={v0:.2f}, A={a0:.2f}")

print(f" Target: P={pt:.2f}, V={vt:.2f}")

print(f" Actual: P={final_state.p:.4f}, V={final_state.v:.4f}, A={final_state.a:.4f}")

print(f" Error : dP={p_err:.4f}, dV={v_err:.4f}, dA={a_err:.4f}")

print("-" * 40)

except Exception as e:

fail_count += 1

print(f"[RUN {i:04d}] EXCEPTION: {str(e)}")

print(f" Start : P={p0:.2f}, V={v0:.2f}, A={a0:.2f}")

print("============================================================")

print(f"SUMMARY: Total {pass_count + fail_count}, Pass {pass_count}, Fail {fail_count}")

print("============================================================")

if __name__ == "__main__":

run_stress_test()