Pybullet: calculateInverseDynamics ignores some joint rotations but not others

[ConnorTingley]

I have been running into an issue where calculateInverseDynamics works on the first joint attached to the body, but not the second joint. Specifically, the angular position of the second joint is ignored.

I put together this example to show the issue:
This is a floating multi-body consisting of 3 links and 2 revolute joints. The base link is the cube in the center.

When the first joint rotates the torque required to accelerate the multibody about the z axis changes, which makes sense because this rotation has changed the moment of inertia in the z direction.

Despite the links being identical and symmetric, the second joint's rotation has 0 impact on the torque required to accelerate the multibody about the z axis.

Here is the code:

import pybullet as p
import time


def TestTorques(bodyID, joint0, joint1):
	p.resetJointState(bodyID, 0, joint0,0)
	p.resetJointState(bodyID, 1, joint1,0)
	return p.calculateInverseDynamics(
		bodyID,
		[0,0,0,1] + [0,0,0] + [joint0, joint1],
		[0]*6 + [0,0], #No angular or linear velocity on the body + no joint velocity
		[0,0,1] + [0,0,0] + [0,0] #Angular acceleration about the z axis of the body. No other accelerations
	)



p.connect(p.GUI)
p.setTimeStep(1/100)
p.setGravity(0, 0, 0)


cubeVis = p.createVisualShape(p.GEOM_BOX, halfExtents = [0.1, 0.1, 0.1])
cubeCol = p.createCollisionShape(p.GEOM_BOX, halfExtents = [0.1, 0.1, 0.1])

brickVis = p.createVisualShape(p.GEOM_BOX, halfExtents = [0.05, 0.2, 0.05])
brickCol = p.createCollisionShape(p.GEOM_BOX, halfExtents = [0.05, 0.2, 0.05])

#Creates a multi-body made up of a cube with 2 rectangular prisms attached on either side. The rectangular prisims rotate with revolute joints
bodyID = p.createMultiBody(baseMass=1,
	baseInertialFramePosition=[0, 0, 0],
	baseCollisionShapeIndex=cubeCol,
	baseVisualShapeIndex=cubeVis,
	basePosition=[0, 0, 2],
	linkMasses = [1,1],
	linkCollisionShapeIndices = [brickCol,brickCol],
	linkVisualShapeIndices = [brickVis,brickVis],
	linkPositions = [[0.15,0,0],[-0.15,0,0]],
	linkOrientations = [[0,0,0,1],[0,0,0,1]],
	linkInertialFramePositions = [[0,0,0],[0,0,0]],
	linkInertialFrameOrientations = [[0,0,0,1],[0,0,0,1]],
	linkParentIndices = [0,0],
	linkJointTypes = [p.JOINT_REVOLUTE,p.JOINT_REVOLUTE],
	linkJointAxis = [[1,0,0],[-1,0,0]]
)

#To accelerate the body about the z axis by 1 rad/s^2
print(TestTorques(bodyID, 0, 0))
#Returns (0.0, 0.0, 0.0, 0.0, 0.0, 0.08, 0.0, 0.0)

time.sleep(3)

#If the first link is rotated by 90 degrees:
print(TestTorques(bodyID, 1.57079632679, 0))
#Returns (0.0, 0.0, 0.0, 0.0, 6.120736075183436e-14, 0.0675, 0.0, 0.0)
#As expected, rotating this joint changes the moment of inertia about the z-axis, changing the required torque

time.sleep(3)

#If the second link is rotated by 90 degrees:
print(TestTorques(bodyID, 0, 1.57079632679))
#Returns (0.0, 0.0, 0.0, 0.0, 0.0, 0.08, 0.0, 0.0)
#Despite the links being identical to each-other and symmetrically positioned, they have different effects on the moment of inertia
#Rotating the second link does not change the inertia (the response is the same as the [0, 0] case)

time.sleep(3)


#Any random rotation of the second link has 0 impact on the response:
print(TestTorques(bodyID, 0, 0.4884))
#Returns (0.0, 0.0, 0.0, 0.0, 0.0, 0.08, 0.0, 0.0)
#For some reason calculateInverseDynamics is ignoring the rotation of the second link

time.sleep(3)

[ConnorTingley]

For anyone having this issue, see this pull request: #4585 (comment)