feat(examples): add ikine_XX timing columns to ik_speed.py

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()