From 227f854d7915541e9795a6de05c2309c958801d3 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 24 Jul 2026 07:47:12 +1000 Subject: [PATCH] fix(examples): repair ik_speed.py after fknm/frne refactor and API renames ik_speed.py predated both the fknm/frne module move (import fknm -> a module path that no longer exists) and the IK solver API rename (ik_lm_chan/ik_nr/ik_gn -> ik_LM(method=...)/ik_NR/ik_GN with renamed kwargs), so it hadn't actually run in some time. Rewrite against the current API, drop stale unused imports, and add modern type hints. Port cpu_info() (added earlier for rne_speed.py) into a shared examples/_cpu_info.py now that ik_speed.py is a second call site. Co-Authored-By: Claude Sonnet 5 --- examples/_cpu_info.py | 59 ++++++++++++++++++++ examples/ik_speed.py | 122 ++++++++++++++++++++++-------------------- examples/rne_speed.py | 55 +------------------ tech-debt.md | 49 +++++++++-------- 4 files changed, 148 insertions(+), 137 deletions(-) create mode 100644 examples/_cpu_info.py diff --git a/examples/_cpu_info.py b/examples/_cpu_info.py new file mode 100644 index 000000000..d4690db9f --- /dev/null +++ b/examples/_cpu_info.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +"""Shared helper for the benchmark scripts in this directory (rne_speed.py, +ik_speed.py) -- factored out once a second script needed the same thing. +""" + +import os +import platform +import subprocess + + +def cpu_info() -> str: + """Best-effort, portable one-line CPU description (name + core count, + clock speed when the OS actually exposes one). No new hard dependency: + psutil is used for clock speed only if already installed, and skipped + otherwise -- some platforms (e.g. Apple Silicon) don't expose a single + meaningful clock speed at all, so a missing/nonsensical reading is + silently omitted rather than shown. + """ + system = platform.system() + name = None + + if system == "Darwin": + try: + name = subprocess.check_output( + ["sysctl", "-n", "machdep.cpu.brand_string"], text=True + ).strip() + except Exception: + pass + elif system == "Linux": + try: + with open("/proc/cpuinfo") as f: + for line in f: + if line.lower().startswith("model name"): + name = line.split(":", 1)[1].strip() + break + except Exception: + pass + elif system == "Windows": + name = platform.processor() or None + + if not name: + name = platform.processor() or platform.machine() or "unknown CPU" + + cores = os.cpu_count() or "?" + info = f"{name} ({cores} cores)" + + try: + import psutil + + freq = psutil.cpu_freq() + # Sanity floor: real clock speeds are in the hundreds-to-thousands + # of MHz; some platforms (Apple Silicon via psutil) report bogus + # single-digit values instead of raising. + if freq and freq.max and freq.max > 100: + info += f", {freq.max:.0f} MHz" + except Exception: + pass + + return info diff --git a/examples/ik_speed.py b/examples/ik_speed.py index b7d82f8ef..ca0c18f27 100644 --- a/examples/ik_speed.py +++ b/examples/ik_speed.py @@ -1,16 +1,17 @@ -import numpy as np -import roboticstoolbox as rtb -import spatialmath as sm -import fknm +#!/usr/bin/env python +"""Benchmark the fast, C-only numerical IK solvers (ik_LM's three variants) +over a batch of random reachable poses. +""" + import time -import swift -import spatialgeometry as sg import sys -from ansitable import ANSITable -from numpy import ndarray -from spatialmath import SE3 -from typing import Union, overload, List, Set +import numpy as np +from ansitable import ANSITable, Column + +import roboticstoolbox as rtb + +from _cpu_info import cpu_info # Our robot and ETS robot = rtb.models.Panda() @@ -18,18 +19,18 @@ ### Experiment parameters # Number of problems to solve -problems = 10000 +nproblems = 10_000 # Cartesion DoF priority matrix -we = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) +mask = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) # random valid q values which will define Tep -q_rand = ets.random_q(problems) +q_rand = ets.random_q(nproblems) # Our desired end-effector poses -Tep = np.zeros((problems, 4, 4)) +Tep = np.zeros((nproblems, 4, 4)) -for i in range(problems): +for i in range(nproblems): Tep[i] = ets.eval(q_rand[i]) # Maximum iterations allowed in a search @@ -43,94 +44,99 @@ solvers = [ - # lambda Tep: ets.ik_nr( - # Tep, - # q0=None, - # ilimit=ilimit, - # slimit=slimit, - # tol=tol, - # reject_jl=False, - # we=we, - # use_pinv=True, - # pinv_damping=0.0, - # ), - # lambda Tep: ets.ik_gn( - # Tep, - # q0=None, - # ilimit=ilimit, - # slimit=slimit, - # tol=tol, - # reject_jl=False, - # we=we, - # use_pinv=False, - # pinv_damping=0.2, - # ), - lambda Tep: ets.ik_lm_chan( + lambda Tep: ets.ik_NR( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=False, + mask=mask, + pinv=True, + pinv_damping=0.0, + ), + lambda Tep: ets.ik_GN( + Tep, + q0=None, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=False, + mask=mask, + pinv=False, + pinv_damping=0.2, + ), + lambda Tep: ets.ik_LM( Tep, q0=None, ilimit=ilimit, slimit=slimit, tol=tol, - reject_jl=True, - we=we, - λ=0.1, + joint_limits=True, + mask=mask, + k=0.1, + method="chan", ), - lambda Tep: ets.ik_lm_wampler( + lambda Tep: ets.ik_LM( Tep, q0=None, ilimit=ilimit, slimit=slimit, tol=tol, - reject_jl=True, - we=we, - λ=1e-4, + joint_limits=True, + mask=mask, + k=1e-4, + method="wampler", ), - lambda Tep: ets.ik_lm_sugihara( + lambda Tep: ets.ik_LM( Tep, q0=None, ilimit=ilimit, slimit=slimit, tol=tol, - reject_jl=True, - we=we, - λ=0.1, + joint_limits=True, + mask=mask, + k=0.1, + method="sugihara", ), ] -times = [] +times: list[float] = [] solver_names = [ - # "Newton Raphson", - # "Gauss Newton", + "Newton Raphson", + "Gauss Newton", "LM Chan", "LM Wampler", "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") + for solver in solvers: - print("Next Solver") + print(".", file=sys.stdout, end="", flush=True) # show activity start = time.time() - for i in range(problems): + for i in range(nproblems): solver(Tep[i]) total_time = time.time() - start times.append(total_time) -print(f"\nNumerical Inverse Kinematics Methods Compared over {problems} problems\n") +print("\r", end="") # clear the progress line table = ANSITable( - "Method", - "Time", + Column("Method", colalign="<", headalign="^"), + Column("Time (μs)", fmt="{:.1f}", headalign="^"), border="thin", ) for name, t in zip(solver_names, times): table.row( name, - (t / problems) * 1e6, + (t / nproblems) * 1e6, ) table.print() diff --git a/examples/rne_speed.py b/examples/rne_speed.py index 6d85c31b8..a0d1ef70d 100644 --- a/examples/rne_speed.py +++ b/examples/rne_speed.py @@ -23,9 +23,6 @@ harness. """ -import os -import platform -import subprocess import timeit import numpy as np @@ -34,57 +31,7 @@ import roboticstoolbox as rtb from roboticstoolbox.robot.Robot import Robot as RobotBase - -def cpu_info() -> str: - """Best-effort, portable one-line CPU description (name + core count, - clock speed when the OS actually exposes one). No new hard dependency: - psutil is used for clock speed only if already installed, and skipped - otherwise -- some platforms (e.g. Apple Silicon) don't expose a single - meaningful clock speed at all, so a missing/nonsensical reading is - silently omitted rather than shown. - """ - system = platform.system() - name = None - - if system == "Darwin": - try: - name = subprocess.check_output( - ["sysctl", "-n", "machdep.cpu.brand_string"], text=True - ).strip() - except Exception: - pass - elif system == "Linux": - try: - with open("/proc/cpuinfo") as f: - for line in f: - if line.lower().startswith("model name"): - name = line.split(":", 1)[1].strip() - break - except Exception: - pass - elif system == "Windows": - name = platform.processor() or None - - if not name: - name = platform.processor() or platform.machine() or "unknown CPU" - - cores = os.cpu_count() or "?" - info = f"{name} ({cores} cores)" - - try: - import psutil - - freq = psutil.cpu_freq() - # Sanity floor: real clock speeds are in the hundreds-to-thousands - # of MHz; some platforms (Apple Silicon via psutil) report bogus - # single-digit values instead of raising. - if freq and freq.max and freq.max > 100: - info += f", {freq.max:.0f} MHz" - except Exception: - pass - - return info - +from _cpu_info import cpu_info robot = rtb.models.DH.Panda() n = robot.n diff --git a/tech-debt.md b/tech-debt.md index da569fb18..b9a4b7772 100644 --- a/tech-debt.md +++ b/tech-debt.md @@ -1277,32 +1277,31 @@ essentially undocumented at the type level, so mismatches like the ## `examples/ik_speed.py` doesn't run — stale imports, predates this session -Found 2026-07-23 while adding a portable CPU-info one-liner to -`examples/rne_speed.py` (a new RNE timing-comparison script) and looking -for a sibling to riff off. `ik_speed.py` fails immediately: `import fknm` -at module scope (line 4) — `fknm` hasn't existed as a top-level importable -module since the fknm/frne refactor moved it to +**FIXED 2026-07-24.** Found 2026-07-23 while adding a portable CPU-info +one-liner to `examples/rne_speed.py` (a new RNE timing-comparison script) +and looking for a sibling to riff off. `ik_speed.py` failed immediately: +`import fknm` at module scope (line 4) — `fknm` hasn't existed as a +top-level importable module since the fknm/frne refactor moved it to `roboticstoolbox.robot.fknm` (merged to `main` 2026-07-03, well before this -session). Not caused by, or related to, the current dynamics-overhaul -branch — pre-existing breakage that had gone unnoticed. - -Also stale, found by inspection while there (none blocking, all part of the -same cleanup): - -- Unused imports: `swift`, `spatialgeometry as sg`, `sys`, `from numpy - import ndarray`, `from typing import Union, overload, List, Set`. -- No type hints (consistent with the `blocks/` entry above — this predates - the modern-syntax convention too). - -**Proposed fix:** update the `fknm` import to `from roboticstoolbox.robot -import fknm` (or import the specific facade functions actually used), -verify the script runs end-to-end again, then sweep the unused imports. -While there, port over the `cpu_info()` helper added to `rne_speed.py` -(portable one-line CPU description — name, core count, clock speed where -the OS exposes one) so `ik_speed.py`'s output reports what machine it ran -on too. Since that would make it the *second* real call site, worth -factoring `cpu_info()` into a small shared `examples/` helper at that point -rather than copy-pasting it again. +session). Not caused by, or related to, the dynamics-overhaul branch — +pre-existing breakage that had gone unnoticed. + +Also stale, beyond the `fknm` import: + +- Unused imports: `swift`, `spatialgeometry as sg`, `sys` (originally), + `from numpy import ndarray`, `from typing import Union, overload, List, + Set`. +- The IK method names themselves had moved on: `ets.ik_lm_chan`/`ik_nr`/ + `ik_gn` no longer exist — current API is `ets.ik_LM(method="chan"/ + "wampler"/"sugihara")`, `ets.ik_NR`, `ets.ik_GN`, with renamed parameters + (`we`→`mask`, `reject_jl`→`joint_limits`, `λ`→`k`, `use_pinv`→`pinv`). + Not caught until the `fknm` import was fixed and the script actually ran. + +**Fix applied:** rewrote `ik_speed.py` against the current `ik_NR`/`ik_GN`/ +`ik_LM` API, added modern type hints, and ported `cpu_info()` — this being +the second real call site, it was factored out into a new shared +`examples/_cpu_info.py` module rather than copy-pasted, and `rne_speed.py` +now imports from there too. Verified end-to-end on both scripts. **Proposed fix:** add type hints throughout `blocks/`, following the same convention as the rest of the codebase (`NDArray`/`ArrayLike` in