feat: add reachability benchmark tools

[HiroIshida]

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, and solver:

python 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:

./batch_bench.sh

Visualize results with animation support:

python visualize.py --direction forward1 --htable 0.29 --animate

Result for current hardware

end-effector down1 direction

end-effector down2 direction

end-effector down2 direction