diff --git a/examples/ik_speed.py b/examples/ik_speed.py index ca0c18f27..a8dad4820 100644 --- a/examples/ik_speed.py +++ b/examples/ik_speed.py @@ -1,6 +1,9 @@ #!/usr/bin/env python -"""Benchmark the fast, C-only numerical IK solvers (ik_LM's three variants) -over a batch of random reachable poses. +"""Benchmark numerical IK solver speed three ways: the fast, C-only ik_XX +solvers; the pure-Python ikine_XX solvers backed by the C-accelerated ETS +(fkine/jacobian evaluated in C++); and ikine_XX backed by the pure-Python +ETS fallback (fkine/jacobian evaluated in Python too, as in a pure-Python +wheel/Pyodide build). """ import time @@ -10,6 +13,7 @@ from ansitable import ANSITable, Column import roboticstoolbox as rtb +import roboticstoolbox.ets.fknm as fknm from _cpu_info import cpu_info @@ -18,9 +22,15 @@ ets = robot.ets() ### Experiment parameters -# Number of problems to solve +# Number of problems to solve for the C-accelerated columns (ik_XX and +# ikine_XX with the C++ ETS) nproblems = 10_000 +# The pure-Python ETS fallback is interpreted at every iteration, not just +# the outer solver loop -- 10,000 problems would take far too long, so use +# a much smaller sample for that column alone. +nproblems_slow = 1_000 + # Cartesion DoF priority matrix mask = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) @@ -43,7 +53,7 @@ tol = 1e-6 -solvers = [ +ik_solvers = [ lambda Tep: ets.ik_NR( Tep, q0=None, @@ -101,7 +111,63 @@ ), ] -times: list[float] = [] +# ikine_XX equivalents -- same settings as above, minus pinv_damping (which +# ikine_NR/ikine_GN don't accept; only the C-only ik_NR/ik_GN do) +ikine_solvers = [ + lambda Tep: ets.ikine_NR( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=False, + mask=mask, + pinv=True, + ), + lambda Tep: ets.ikine_GN( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=False, + mask=mask, + pinv=False, + ), + lambda Tep: ets.ikine_LM( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=True, + mask=mask, + k=0.1, + method="chan", + ), + lambda Tep: ets.ikine_LM( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=True, + mask=mask, + k=1e-4, + method="wampler", + ), + lambda Tep: ets.ikine_LM( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=True, + mask=mask, + k=0.1, + method="sugihara", + ), +] solver_names = [ "Newton Raphson", @@ -111,32 +177,70 @@ "LM Sugihara", ] -print(f"\nNumerical Inverse Kinematics Methods benchmark:\n * running on {cpu_info()},\n * robot is {robot.name} with {robot.n} DoF,\n * for a batch of {nproblems} random configurations.\n\nTime per IK solution:\n") +print( + f"\nNumerical Inverse Kinematics Methods benchmark:\n" + f" * running on {cpu_info()},\n" + f" * robot is {robot.name} with {robot.n} DoF,\n" + f" * ik_XX and ikine_XX (C++ ETS) columns use {nproblems} random configurations,\n" + f" * ikine_XX (pure Python ETS) column uses {nproblems_slow} (too slow at {nproblems}).\n" + f"\nTime per IK solution:\n" +) + +ik_times: list[float] = [] +ikine_cpp_times: list[float] = [] +ikine_py_times: list[float] = [] -for solver in solvers: - print(".", file=sys.stdout, end="", flush=True) # show activity +for solver in ik_solvers: + print(".", file=sys.stdout, end="", flush=True) # show activity start = time.time() + for i in range(nproblems): + solver(Tep[i]) + ik_times.append(time.time() - start) + +for solver in ikine_solvers: + print(".", file=sys.stdout, end="", flush=True) + start = time.time() for i in range(nproblems): solver(Tep[i]) + ikine_cpp_times.append(time.time() - start) + +# Force the pure-Python ETS fallback: a fresh ETS starts with no C++ handle +# built, and robot.ets() returns a cached instance, so dirty this one's +# cache while _C_AVAILABLE is patched off to make it rebuild as pure-Python. +fknm._C_AVAILABLE = False +ets._fknm_stale = True - total_time = time.time() - start - times.append(total_time) +for solver in ikine_solvers: + print(".", file=sys.stdout, end="", flush=True) + + start = time.time() + for i in range(nproblems_slow): + solver(Tep[i]) + ikine_py_times.append(time.time() - start) +fknm._C_AVAILABLE = True +ets._fknm_stale = True -print("\r", end="") # clear the progress line +print("\r", end="") # clear the progress line table = ANSITable( Column("Method", colalign="<", headalign="^"), - Column("Time (μs)", fmt="{:.1f}", headalign="^"), + Column("ik_XX (μs)", fmt="{:.1f}", headalign="^"), + Column("ikine_XX, C++ ETS (μs)", fmt="{:.1f}", headalign="^"), + Column("ikine_XX, pure Python ETS (μs)", fmt="{:.1f}", headalign="^"), border="thin", ) -for name, t in zip(solver_names, times): +for name, ik_t, ikine_cpp_t, ikine_py_t in zip( + solver_names, ik_times, ikine_cpp_times, ikine_py_times +): table.row( name, - (t / nproblems) * 1e6, + (ik_t / nproblems) * 1e6, + (ikine_cpp_t / nproblems) * 1e6, + (ikine_py_t / nproblems_slow) * 1e6, ) table.print()