[PyBullet] getNumJoints() does not work when using loadBullet()

[manas96]

Hi, I'm trying to load a bullet scene that consists of two rods connected by a single joint like so:

# This creates a .bullet file
import time

import pybullet as pb
import pybullet_data
import numpy as np
from pybullet import JOINT_POINT2POINT, GEOM_SPHERE

cid = pb.connect(pb.GUI, options='--mouse_wheel_multiplier=0.400000')
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
ground = pb.loadURDF("plane100.urdf",useMaximalCoordinates=True)
pb.setGravity(0, 0, -10)

BOX_LENGTH = 1
BOX_HE = np.array([BOX_LENGTH*0.01, BOX_LENGTH*0.01, BOX_LENGTH/2])
BOX_MASS = 1
RED = [1, 0, 0, 1]
ORIENTATION = pb.getQuaternionFromAxisAngle([0, 0, 0], 0)

BOX_CS = pb.createCollisionShape(pb.GEOM_BOX, halfExtents=BOX_HE)
BOX_VS = pb.createVisualShape(shapeType=pb.GEOM_BOX, halfExtents=BOX_HE,
                                  rgbaColor=RED)

b1 =  pb.createMultiBody(BOX_MASS, BOX_CS, BOX_VS,
                                    basePosition=[0,0,0.5],
                                    baseOrientation=ORIENTATION,
                                    useMaximalCoordinates=True)

b2 =  pb.createMultiBody(BOX_MASS, BOX_CS, BOX_VS,
                                    basePosition=[0,0,1.5],
                                    baseOrientation=ORIENTATION,
                                    useMaximalCoordinates=True)

joint = pb.createConstraint(parentBodyUniqueId=b2, parentLinkIndex=-1,
                                childBodyUniqueId=b1, childLinkIndex=-1,
                                jointType=JOINT_POINT2POINT, jointAxis=[0, 0, 0],
                                parentFramePosition=[0, 0, -BOX_LENGTH / 2],
                            childFramePosition=[0, 0, BOX_LENGTH / 2])

pb.saveBullet("getNumJoints.bullet")

paused=True
SPACE = ord(' ')
while 1:
    keys = pb.getKeyboardEvents()
    if SPACE in keys and keys[SPACE] & pb.KEY_WAS_TRIGGERED:
        paused = not paused
    if not paused:
        time.sleep(1 / 240.)
        pb.stepSimulation()

# This loads the .bullet file from above
import time
import pybullet as pb
import pybullet_data

cid = pb.connect(pb.GUI)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
pb.setGravity(0, 0, -10)

objs = pb.loadBullet('getNumJoints.bullet')

print(f'objects: {len(objs)}')

for i in range(len(objs)):
    print(f'joints for body {i}: {pb.getNumJoints(bodyUniqueId=i)}')

paused = True
SPACE = ord(' ')
while 1:
    keys = pb.getKeyboardEvents()

    if SPACE in keys and keys[SPACE] & pb.KEY_WAS_TRIGGERED:
        paused = not paused

    if not paused:
        time.sleep(1 / 240.)
        pb.stepSimulation()

However, no joints are detected for any body in the loaded scene, as this output shows:

objects: 3
joints for body 0: 0
joints for body 1: 0
joints for body 2: 0

The actual scene loads just fine, and it has a joint in the correct place. However, I need access to the joint info programmatically. I dug around the C++ code and it seems that the BodyJointInfoCache** bodyJointsPtr in PhysicsClientSharedMemory::getNumJoints() is evaluating to null.

Is this a bug or am I doing something wrong?

Also note that I need to use useMaximalCoordinates=True otherwise pb.loadBullet() does not return object IDs.

[manas96]

On further thought, I should be using pb.getNumConstraints() in the loading script (since I am creating a user-defined constraint). However, this too returns 0 when it should be returning 1.

It seems that during loadBullet() the function btWorldImporter::convertConstraintDouble() is called, which creates a rigidbody p2p constraint (btPoint2PointConstraint). It should be creating a multibody p2p constraint (btMultiBodyPoint2Point) instead, right? I'd be happy to work out a fix with PR if my assessment is correct :)