Improving the 1D search algorithm in IK-Geo to achieve high precision and success rate. Testing is performed on the Motoman SIA50D 7-DOF robot arm parameterized by conventional SEW angle with shoulder placed on joint 2.
The goal was to achieve 100% empirical success rate over 10,000 random test cases (random joint angles) while minimizing execution time based on the following milestones:
- 20 Hz, 50 ms – GUI display
- 100 Hz, 10 ms – Mid-range control loop
- 500 Hz, 2 ms – Real-time control loop (stretch goal)
Testing was performed using MATLAB MEX on a computer running Windows 10 on an Intel Core i7-3770K CPU at 3.50 GHz and 16 GB memory.
With just a few improvements, 1D search performance was significantly improved. Success rate went from 89% to 100%, and computation time was better than the fastest milestone.
 |
 |
|---|
| Revision | Note | % Correct | Time (us) |
|---|---|---|---|
| 0 | 89 | 1235.55 | |
| 1 | Search over 2/4 branches | 89 | 800.56 |
| 2 | LS solns, increase cross thresh, decrease search tol | 99.8 | 1158.93 |
| 3 | Cut initial samples from 1e3 to 500 | 99.8 | 656.318 |
| 3* | * Increase test cases from 1,000 to 10,000 | 99.78 | 735.829 |
| 4 | Simplify zero cross detection code, fix NaN branches | 99.78 | 715.402 |
| 5 | Find local min / max for each triangle pointing to 0 | 100 | 754.681 |
| 5* | * Increase test cases from 10,000 to 100,000 | 99.999 | 791.278 |
| 6 | Decrease samples from 500 to 250 (10,000 test cases) | 100 | 497.381 |
The following improvements were made:
- Search over only 2 of 4 error function branches. Due to robot symmetry, the four error function branches come in two identical pairs.
- Use least-squares solutions. This guarantees the error function is always defined, meaning the bracketing method does not fail near endpoints.
- Search not just for zeros but also for minima / maxima. This improves performance near robot singularities where the error function touches but only barely passes zero.
There are still a number of strategies that may improve 1D search performance even further:
- Use multiple unique error functions
- Hard-code kinematic parameters into code
- Or hard code error function with tan(𝜃/2)
- Use intelligent global optimization algorithms (not uniform sampling)
- Search for endpoints
- Or find endpoints analytically
Similar changes were made to the C++ code in the main IK-Geo repository.
Timing is much slower for C++ (but still faster than the stretch goal). It's possible that the testing method is what's slowing down the code.
| Revision | Note | % Correct | Time (us) |
|---|---|---|---|
| 0 | 65 | ? | |
| 1 | Increased samples to 1e3 to match MATLAB rev0 | 89 | 1718.24 |
| 2 | Only use 2 of 4 branches to finds zeros | 89 | 979.174 |
| 3 | LS solns, don't use cross thresh | 89 | 2007.35 |
| 4 | Cut initial samples from 1e3 to 500 | 96 | 1263.59 |
| 5 | Fix broken false position method | 99.32 | 1227.1 |
| 6 | Fix broken wrap_to_pi function | 99.92 | 1264.86 |
| 7 | Min / max for root finding | 99.87 | 1315.02 |
| 8 | Fix min/max logic, reduce initial samples 500->250 | 100 | 907.286 |