Bug report: Sometimes pybullets ClosestPoints interchanges position_on_a and position_on_b

[ichumuh]

Here is a small example

from collections import namedtuple

import pybullet as p
import pybullet_data

ContactInfo = namedtuple('ContactInfo', ['contact_flag', 'body_unique_id_a', 'body_unique_id_b', 'link_index_a',
                                          'link_index_b', 'position_on_a', 'position_on_b', 'contact_normal_on_b',
                                          'contact_distance', 'normal_force', 'lateralFriction1',
                                          'lateralFrictionDir1',
                                          'lateralFriction2', 'lateralFrictionDir2'])

physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-10)

cubeStartOrientation = p.getQuaternionFromEuler([-1,0,0])
r2d2_a = p.loadURDF("r2d2.urdf", [1.0, 0.0, 0.0], cubeStartOrientation)
r2d2_b = p.loadURDF("r2d2.urdf", [-1.0, 0.0, 0.0], cubeStartOrientation)

closest_points = p.getClosestPoints(r2d2_a, r2d2_b, 10.0)

for point in closest_points:
    contact = ContactInfo(*point)
    if r2d2_a == contact.body_unique_id_a and contact.position_on_a[0] < 0.0:
        print('positionOnA and positionOnB interchanged')
    else:
        print('everything is fine')

p.disconnect()

Since r2d2_a is in the positive x area, all position_on_a[0] values should be positive. Which means that print('positionOnA and positionOnB interchanged') should never get printed. However, sometimes position_on_a and position_on_b and the contact normal direction appear to be swapped, such that they do not match with the returned body_unique_id_a, body_unique_id_b and so on, cause the if to get triggered.
Can you please have a look into this?
Edit:
I'm using pybullet version 2.8.7.

[erwincoumans]

Thanks for the report, it should be fixed in master
#3026
Will be uploaded to pypi/pip soon.

[erwincoumans]

By the way, you can speed up the urdf loading by temporary disabling rendering:

p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
r2d2_a = p.loadURDF("r2d2.urdf", [1.0, 0.0, 0.0], cubeStartOrientation)
r2d2_b = p.loadURDF("r2d2.urdf", [-1.0, 0.0, 0.0], cubeStartOrientation)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

[ichumuh]

Thanks for the quick response @erwincoumans! The scenario I've posted seems fixed now. However, it seems like it can also happen when m_bodyUniqueIdA == m_bodyUniqueIdB, which your new check doesn't catch. So the result would be points that are swapped with respect to the links_ids. Unfortunately, its more difficult to setup a simple example, that showcases this. But if it helps, I'll do it.