yck011522
commented
2 weeks ago 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 * thresholdWhen 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: 39Now 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.
This PR fixes a few problems I encountered while writing unit tests for PyBullet. The problems are:
Fixed error in
PyBulletForwardKinematics.forward_kinematicswhere function would crash ifoptionswas not passed.Fixed error in
PyBulletInverseKinematics._accurate_inverse_kinematicswhere threshold was not squared for comparison.Added
PyBulletClient.load_existing_robotto load from an existing Robot, such as those in RobotLibrary. This makes writing tests much easier.Added unit test for
PyBulletClientandPyBulletPlannerbackend features, including Ik and FK agreement tests.This should be reviewed after #428 and earlier PRs are merged to main.