Inconsistency between rayTest and rayTestBatch of pybullet

[HiroIshida]

I found the case when rayTest and rayTestBatch yield inconsistent results. There is an example code performing batch ray cast toward r2d2. Based on that, I modified code to test the results of rayTest and rayTestBatch are consistent. I conducted the test also for sphere_small, sphere2, block, and humanoid as well as r2d2. (note that all these data is provided in pybullet_data, so one can just run the following code if has pybullet.) The script below does that..

import pybullet as p
import math
import pybullet_data

def single_test(urdf_file, isDIRECT=True):
    p.connect(p.DIRECT if isDIRECT else p.GUI)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.loadURDF(urdf_file, [3, 3, 1])

    numRays = 1024; rayLen = 13

    info_lst = []; rayFrom_lst = []; rayTo_lst = []

    for i in range(numRays):
        rayFrom = [0, 0, 1]
        rayTo = [rayLen * math.sin(2. * math.pi * float(i) / numRays),
                rayLen * math.cos(2. * math.pi * float(i) / numRays), 1]

        info = p.rayTest(rayFrom, rayTo)
        info_lst.append(info[0])

        p.addUserDebugLine(rayFrom, rayTo, [1, 0, 0])
        rayFrom_lst.append(rayFrom)
        rayTo_lst.append(rayTo)

    info_lst_batch = p.rayTestBatch(rayFrom_lst, rayTo_lst)

    collide_predicate = lambda info:info[0]!=-1
    info_filtered = list(filter(collide_predicate, info_lst))
    info_batch_filtered = list(filter(collide_predicate, info_lst_batch))

    p.disconnect()

    return info_filtered, info_batch_filtered

def isConsistent(model):
    info_filtered, info_batch_filtered = single_test(model)
    # number of collide ray must be equal 
    return (len(info_filtered) == len(info_batch_filtered)) 

models = ["sphere_small.urdf", "sphere2.urdf", "block.urdf",  # composed of single object
        "r2d2.urdf", "humanoid/humanoid.urdf" # composed of multiple objects 
        ]

lst_consistensy = [isConsistent(model) for model in models]

As a result, we get lst_consistensy = [true, true, true, false, false]. Here I check the consistency by comparing number of collision ray of the both methods. The result is strange, because for all urdf-models composed of single object, we can observe the consistency. However for those composed of multiple links, we cannot.

My environment is Ubuntu16.04, and tested python3.7.7 after building from source at fa21872.

[Steven89Liu]

from the function name pybullet_rayTestObsolete the rayTest should be obsolete, we should use the rayTestBatch instead. i also check the code that rayTestBatch adds some new parameters but don't be initialized in the old function.