Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 59 additions & 0 deletions examples/_cpu_info.py
Original file line number Diff line number Diff line change
@@ -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
122 changes: 64 additions & 58 deletions examples/ik_speed.py
Original file line number Diff line number Diff line change
@@ -1,35 +1,36 @@
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()
ets = robot.ets()

### 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
Expand All @@ -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()
55 changes: 1 addition & 54 deletions examples/rne_speed.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,6 @@
harness.
"""

import os
import platform
import subprocess
import timeit

import numpy as np
Expand All @@ -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
Expand Down
49 changes: 24 additions & 25 deletions tech-debt.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down