1D Search Improvements

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.

Problem formulation

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.

Results

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.

Future work

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

C++ Results

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