Tests for PyBulletClient and PyBulletPlanner

[yck011522]

This PR fixes a few problems I encountered while writing unit tests for PyBullet. The problems are:

• Fixed error in PyBulletForwardKinematics.forward_kinematics where function would crash if options was not passed.

• Fixed error in PyBulletInverseKinematics._accurate_inverse_kinematics where threshold was not squared for comparison.

• Added PyBulletClient.load_existing_robot to load from an existing Robot, such as those in RobotLibrary. This makes writing tests much easier.

• Added unit test for PyBulletClient and PyBulletPlanner backend features, including Ik and FK agreement tests.

This should be reviewed after #428 and earlier PRs are merged to main.

[yck011522]

One interesting error I found after fixing the error calculation method inside _accurate_inverse_kinematics. In some cases the previous default error threshold of (1e-06) is too fine for the Pybullet engine to converge.

The new logic of comparison are as follows:

            # The distance is squared to avoid a sqrt operation
            distance_square = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]
            # Therefor, the threshold is squared as well
            close_enough = distance_square < threshold * threshold

When threshold is 1e-06, the "threshold * threshold" requires is therefor 1e-12. The following printout statement shows that the Pybullet engine will bring the squared distance to about 6e-09 in 10 iterations, to 2e-09 in 20 iterations and 3e-10 in 40 iterations. Now

Distance square: 0.06783849015301963, iter: 0
Distance square: 0.04080386272892813, iter: 1
Distance square: 0.0012774338260042929, iter: 2
Distance square: 4.1008395976627164e-05, iter: 3
Distance square: 3.949337188835485e-06, iter: 4
Distance square: 4.2369392675433764e-07, iter: 5
Distance square: 4.6010113585735485e-08, iter: 6
Distance square: 8.654918026706318e-09, iter: 7
Distance square: 7.734573026495924e-09, iter: 8
Distance square: 6.912580430284333e-09, iter: 9
Distance square: 6.1778495763492064e-09, iter: 10
Distance square: 5.520494250620936e-09, iter: 11
Distance square: 4.93373409904353e-09, iter: 12
Distance square: 4.408796572263381e-09, iter: 13
Distance square: 3.9412074048778695e-09, iter: 14
Distance square: 3.522045999293051e-09, iter: 15
Distance square: 3.1475359762995856e-09, iter: 16
Distance square: 2.8132310370128422e-09, iter: 17
Distance square: 2.5143298665316064e-09, iter: 18
Distance square: 2.2472464777930297e-09, iter: 19
Distance square: 2.0076647899710393e-09, iter: 20
Distance square: 1.7946989672165178e-09, iter: 21
Distance square: 1.6037330241577508e-09, iter: 22
Distance square: 1.4332313669204578e-09, iter: 23
Distance square: 1.2810321359243987e-09, iter: 24
Distance square: 1.1451238624002485e-09, iter: 25
Distance square: 1.0234150971119875e-09, iter: 26
Distance square: 9.143183654633948e-10, iter: 27
Distance square: 8.172783783064749e-10, iter: 28
Distance square: 7.304872817746188e-10, iter: 29
Distance square: 6.526568285812016e-10, iter: 30
Distance square: 5.83357939731668e-10, iter: 31
Distance square: 5.212664633267759e-10, iter: 32
Distance square: 4.66026550682674e-10, iter: 33
Distance square: 4.1657881588703397e-10, iter: 34
Distance square: 3.721999153326225e-10, iter: 35
Distance square: 3.325933750629431e-10, iter: 36
Distance square: 2.9724049888089086e-10, iter: 37
Distance square: 2.655496822433339e-10, iter: 38
Distance square: 2.3747267485762255e-10, iter: 39

Now we had a completely wrong comparison in the past, so we didn't catch such error. But now that the comparision is fixed, the previous default 20 iterations will not bring us down to threshold of 1e-06.

This is all a balance of accuracy vs computing time, I think considering the 10th iteration resulted in 6e-09, equivalent to 7.7e-05 meters or 0.077 mm in real live. I suggest changing the default IK accuracy threshold to 0.1mm, which is already much better than many industrial robot accuracy.
For example ABB with Absolute Accuracy package claims to bring accuracy down to 0.5mm.
https://library.e.abb.com/public/0f879113235a0e1dc1257b130056d133/Absolute%20Accuracy%20EN_R4%20US%2002_05.pdf

Product concept
The difference between a virtual robot and a real robot can be typically 8-15 mm, resulting from mechanical tolerances and deflection in the robot structure. The introduction of the ABB Absolute Accuracy concept bridges this gap with a complete accuracy concept for the entire robot lifetime, ensuring a maintainable accuracy of approx 0.5 mm in the entire working range. The Product concept comprises three interconnected aspects.

[yck011522]

Not to mention 1e-06 is a micrometer and according to wikipedia, the length of a typical bacterium is 1 to 10 micrometer...

[yck011522]

Note that the new Pybullet Tests are skipped in the IronPython environment. This is done by adding them to the exclude list in the ipy_test_runner.py