feat: add reachability benchmark tools

Signed-off-by: Hirokazu Ishida (SB Intuitions) <hirokazu.ishida@sbintuitions.co.jp>

Author: HiroIshida

reachability_bench/.gitignore

@@ -0,0 +1 @@
+/result

reachability_bench/.python-version

@@ -0,0 +1 @@
+3.10

reachability_bench/README.md

@@ -0,0 +1,21 @@
+# OpenArm Reachability Benchmark
+
+This tool benchmarks the reachability workspace of the OpenArm robot for different end effector orientations.
+This tool is designed to visualize the current reachability given the URDF file and leverage it for future improvements and customization of the OpenArm's mechanical structure.
+
+## Usage
+Run a single benchmark by specifying height, direction
+```bash
+uv run bench.py --z 0.5 --direction forward1
+```
+z is the height of the target coordinates, and the reachability map is computed for the plane at height z.
+
+Execute batch benchmarks across multiple heights and directions:
+```bash
+./batch_bench.sh
+```
+
+Visualize results with animation support:
+```bash
+uv run visualize.py --direction forward1 --htable 0.29 --animate
+```

reachability_bench/activate.sh

@@ -0,0 +1 @@
+source .venv/bin/activate

reachability_bench/batch_bench.sh

@@ -0,0 +1 @@
+parallel --bar uv run bench.py --z {1} --direction {2} ::: 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 ::: forward1 forward2 down1 down2

reachability_bench/bench.py

@@ -0,0 +1,90 @@
+# Copyright 2025 SB Intuitions Corp.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+from typing import Literal
+
+import numpy as np
+import tqdm
+import tyro
+from plainmp.ik import solve_ik_srinv
+from plainmp.robot_spec import OpenArmV10RarmSpec
+from skrobot.coordinates import Coordinates
+from threadpoolctl import threadpool_limits
+
+
+class PlainmpIKSolver:
+    def __init__(self):
+        self.spec = OpenArmV10RarmSpec()
+
+    def solve(self, tcp_coords_ros: Coordinates, n_global_iter: int) -> np.ndarray | None:
+        cst = self.spec.create_tcp_pose_const(tcp_coords_ros)
+        lb, ub = self.spec.angle_bounds()
+        ret = solve_ik_srinv(cst, lb, ub, q_seed=None, max_trial=n_global_iter)
+        if ret.success:
+            return ret.q
+        return None
+
+
+def benchmark(
+    z: float,
+    direction: Literal["forward1", "forward2", "down1", "down2"] = "forward1",
+):
+    if direction == "forward1":
+        co_base = Coordinates()
+    elif direction == "forward2":
+        co_base = Coordinates()
+        co_base.rotate(-np.pi * 0.5, "x")
+    elif direction == "down1":
+        co_base = Coordinates()
+        co_base.rotate(np.pi * 0.5, "y")
+    elif direction == "down2":
+        co_base = Coordinates()
+        co_base.rotate(np.pi * 0.5, "y")
+        co_base.rotate(-np.pi * 0.5, "x")
+    else:
+        assert False
+
+    result_dir = Path(__file__).parent / "result"
+    result_dir.mkdir(exist_ok=True)
+
+    xlin = np.linspace(0.0, 0.8, 20)
+    ylin = np.linspace(-0.5, 0.5, 20)
+    X, Y = np.meshgrid(xlin, ylin)
+    points2d = np.stack([X.ravel(), Y.ravel()], axis=1)
+    solver = PlainmpIKSolver()
+
+    with threadpool_limits(limits=1, user_api="blas"):
+        qs_reachable = []
+        labels = []
+        for point2d in tqdm.tqdm(points2d):
+            point3d = np.hstack([point2d, z])
+            co = co_base.copy_worldcoords()
+            co.translate(point3d, wrt="world")
+            ret = solver.solve(co, 30)
+            success = ret is not None
+            labels.append(success)
+            if success:
+                qs_reachable.append(ret)
+        is_reachables = np.array(labels)
+        qs_reachable = np.array(qs_reachable)
+        pts_reachable = points2d[is_reachables]
+
+        d = {"z": z, "pts": pts_reachable, "qs": qs_reachable}
+        with (result_dir / f"{direction}-z-{round(z, 2)}.npz").open(mode="wb") as f:
+            np.savez(f, d)
+
+
+if __name__ == "__main__":
+    tyro.cli(benchmark)

reachability_bench/format.sh

@@ -0,0 +1,2 @@
+ruff check . --fix
+ruff format .

reachability_bench/pyproject.toml

@@ -0,0 +1,26 @@
+[project]
+name = "reachability-test"
+version = "0.1.0"
+description = "Add your description here"
+readme = "README.md"
+requires-python = ">=3.10"
+dependencies = [
+    "matplotlib>=3.10.7",
+    "plainmp>=0.3.4",
+    "plotly>=6.5.0",
+    "scikit-robot>=0.2.35",
+    "threadpoolctl>=3.6.0",
+    "tqdm>=4.67.1",
+    "tyro>=0.9.35",
+]
+
+[tool.ruff]
+ignore = ["E731"]
+lint.select = ["E", "F", "I"]
+line-length = 100
+
+[dependency-groups]
+dev = [
+    "pudb>=2025.1.3",
+    "ruff>=0.14.6",
+]

reachability_bench/uv.lock

@@ -0,0 +1,89 @@
+# Copyright 2025 SB Intuitions Corp.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+from pathlib import Path
+from typing import Literal
+
+import matplotlib.pyplot as plt
+import numpy as np
+import tyro
+from plainmp.robot_spec import OpenArmV10RarmSpec
+from skrobot.model.primitives import Box
+from skrobot.viewers import PyrenderViewer
+
+
+def main(
+    htable: float = 0.29,
+    hmin: float | None = None,
+    hmax: float | None = None,
+    animate: bool = False,
+    direction: Literal["forward1", "forward2", "down1", "down2"] = "forward1",
+):
+    feasible_pointss = []
+    qss = []
+    if hmin is None:
+        hmin = -np.inf
+    if hmax is None:
+        hmax = np.inf
+    z_point_max = -np.inf
+    for result_path in Path("result").iterdir():
+        if not str(result_path.stem).startswith(direction):
+            continue
+        npz = np.load(result_path, allow_pickle=True)
+        dic = npz["arr_0"].item()
+        z = dic["z"]
+        bools = dic["pts"]
+        qs = dic["qs"]
+        z_point_max = max(z_point_max, z)
+        if z > htable and z > hmin and z < hmax:
+            if len(bools) > 0:
+                feasible_pointss.append([z, bools])
+                qss.append(qs)
+
+    qs = np.vstack(qss)
+
+    v = PyrenderViewer()
+    spec = OpenArmV10RarmSpec()
+    model = spec.get_robot_model(with_mesh=True)
+    v.add(model)
+    table_thickness = 0.03
+    table = Box([0.4, 1.0, table_thickness], face_colors=[0.65, 0.45, 0.0, 1.0])
+    table.translate([0.3, 0.0, htable - table_thickness * 0.5])
+    v.add(table)
+
+    cmap = plt.get_cmap("jet")
+
+    for z, points in feasible_pointss:
+        for p_2d in points:
+            p_3d = np.hstack([p_2d, z])
+            size = 0.01
+            s = (z - htable) / (z_point_max - htable)
+            color = cmap(s)
+            box = Box([size, size, size], face_colors=color)
+            box.translate(p_3d)
+            v.add(box)
+    v.show()
+
+    if animate:
+        input("press enter to start animate")
+        for q in qs:
+            spec.set_skrobot_model_state(model, q)
+            v.redraw()
+            time.sleep(0.5)
+    time.sleep(1000)
+
+
+if __name__ == "__main__":
+    tyro.cli(main)