Bug report: LINK_FRAME and WORLD_FRAME inverted in applyExternalTorque in pyBullet

[charles-blouin]

Issue: The flag LINK_FRAME and WORLD_FRAME seem inverted in the function applyExternalTorque in pybullet.

How to replicate:

import os,  inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import time
import pybullet as p
import pybullet_data

p.connect(p.GUI)
timeStep = 0.01
p.setRealTimeSimulation(0)
p.setTimeStep(timeStep)

p.resetSimulation()
cube0 = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"),[0,0,0])
cube1 = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"),[2,0,0], [0, 0.7071, 0, 0.7071])
cube2 = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"),[0,2,0])
cube3 = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"),[2,2,0], [0, 0.7071, 0, 0.7071])


while 1:
    p.stepSimulation()
    time.sleep(timeStep)
    p.applyExternalTorque(cube0, -1, [1, 0, 0], p.LINK_FRAME)
    p.applyExternalTorque(cube1, -1, [1, 0, 0], p.LINK_FRAME)
    p.applyExternalTorque(cube2, -1, [1, 0, 0], p.WORLD_FRAME)
    p.applyExternalTorque(cube3, -1, [1, 0, 0], p.WORLD_FRAME)

Note that the function applyExternalForce is behaving correctly. The issue was already reported here:
https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=11590

I am not sure where is the bug. The flags in python.c seem correct at this line. Maybe someone could try to replicate the issue in C++.

[rubendsa]

I also have this issue with external torque. Right now, it seems to only be an issue when applying a link_frame torque on the base link. ex: p.applyExternalTorque(robotId, -1, [10,0,0], 1)

[JacopoPan]

Has this eventually been fixed?
Or are WORLD_FRAME and LINK_FRAME still swapped for .applyExternalTorque() (to the base)?

Minimal reproducible example (from this work https://github.com/JacopoPan/gym-pybullet-drones):

import time
import numpy as np
import pybullet as p
import pybullet_data

DURATION_SEC = 10
RESTARTS = 0

########################################################################################################################
# applyExternalTorque()'s WORLD_FRAME and LINK_FRAME appear to be swapped: github.com/bulletphysics/bullet3/issues/1949 
########################################################################################################################

if __name__ == "__main__":

	print(p.getQuaternionFromEuler([0,0,0]))

	START = time.time()
	PYB_CLIENT = p.connect(p.GUI, key=0)
	p.resetDebugVisualizerCamera(cameraDistance=2, cameraYaw=-30, cameraPitch=-30, cameraTargetPosition=[0.0,0.0,0.0], physicsClientId=PYB_CLIENT)
	p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=PYB_CLIENT)
	ROBOT_ID = p.loadURDF("duck_vhacd.urdf",[0,0,0.1], p.getQuaternionFromEuler([0,0,0]), physicsClientId=PYB_CLIENT)
	X_AX = -1; Y_AX = -1; Z_AX = -1; 
	#### Make the drone weightless #########################################################################################
	p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
	#### Set initial pose ##################################################################################################
	# p.resetBasePositionAndOrientation(ROBOT_ID, posObj=[-.7,-.5,.3], ornObj=p.getQuaternionFromEuler([0,0,0], physicsClientId=PYB_CLIENT), physicsClientId=PYB_CLIENT)
	# p.resetBasePositionAndOrientation(ROBOT_ID, posObj=[-.7,-.5,.3], ornObj=p.getQuaternionFromEuler([0,0,60*(np.pi/180)], physicsClientId=PYB_CLIENT), physicsClientId=PYB_CLIENT)
	p.resetBasePositionAndOrientation(ROBOT_ID, posObj=[-.7,-.5,.3], ornObj=p.getQuaternionFromEuler([0,60*(np.pi/180),0], physicsClientId=PYB_CLIENT), physicsClientId=PYB_CLIENT)
	########################################################################################################################
	for i in range(DURATION_SEC*240):
		X_AX = p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0.1,0,0],lineColorRGB=[1,0,0],parentObjectUniqueId=ROBOT_ID, parentLinkIndex=-1, replaceItemUniqueId=X_AX, physicsClientId=PYB_CLIENT)
		Y_AX = p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0.1,0],lineColorRGB=[0,1,0],parentObjectUniqueId=ROBOT_ID, parentLinkIndex=-1, replaceItemUniqueId=Y_AX, physicsClientId=PYB_CLIENT)
		Z_AX = p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0.1],lineColorRGB=[0,0,1],parentObjectUniqueId=ROBOT_ID, parentLinkIndex=-1, replaceItemUniqueId=Z_AX, physicsClientId=PYB_CLIENT)
		#### Thrust ############################################################################################################
		# p.applyExternalForce(ROBOT_ID, -1, forceObj=[0.,0.,1e-2], posObj=[0.,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) # CORRECT
		# p.applyExternalForce(ROBOT_ID, -1, forceObj=[0.,0.,1e-2], posObj=[0.,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # INCORRECT
		#### Roll ##############################################################################################################
		p.applyExternalTorque(ROBOT_ID, -1, torqueObj=[5e-4,0.,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # CORRECT ?
		# p.applyExternalTorque(ROBOT_ID, -1, torqueObj=[5e-4,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) # INCORRECT ?
		#### Pitch #############################################################################################################
		# p.applyExternalTorque(ROBOT_ID, -1, torqueObj=[0.,5e-4,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # CORRECT ?
		# p.applyExternalTorque(ROBOT_ID, -1, torqueObj=[0.,5e-4,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) # INCORRECT ?
		#### Yaw ###############################################################################################################
		# p.applyExternalTorque(ROBOT_ID, -1, torqueObj=[0.,0.,5e-4], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # CORRECT ?
		# p.applyExternalTorque(ROBOT_ID, -1, torqueObj=[0.,0.,5e-4], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) # INCORRECT ?
		########################################################################################################################
		p.stepSimulation(physicsClientId=PYB_CLIENT)
		pos, quat = p.getBasePositionAndOrientation(ROBOT_ID, physicsClientId=PYB_CLIENT)
		rpy = p.getEulerFromQuaternion(quat)
		vel, ang_v = p.getBaseVelocity(ROBOT_ID, physicsClientId=PYB_CLIENT)
		print("--- step number {:d}".format(i), 
			"--- X {:.2f} Y {:.2f} Z {:.2f} vel. {:.2f}".format(pos[0],pos[1],pos[2], np.linalg.norm(vel)), 
			"--- roll {:.2f} pitch {:.2f} yaw {:.2f}".format(rpy[0]*(180/np.pi), rpy[1]*(180/np.pi), rpy[2]*(180/np.pi)),
			"--- ang. vel. {:.2f} {:.2f} {:.2f} --- ".format(ang_v[0]*(180/np.pi), ang_v[1]*(180/np.pi), ang_v[2]*(180/np.pi)), end="\r")
		elapsed = time.time()-START
		if elapsed < i/240: time.sleep(i/240-elapsed)	
		if i>1 and i%((DURATION_SEC/(RESTARTS+1))*240)==0: p.resetSimulation(physicsClientId=PYB_CLIENT)

[JacopoPan]

I think this might result from this if branch https://github.com/bulletphysics/bullet3/blob/master/examples/SharedMemory/PhysicsServerCommandProcessor.cpp#L11055

if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
    btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis() * torqueLocal;
    mb->addBaseTorque(torqueWorld);
    //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}

I think the bitwise comparison to assign isLinkFrame on line 11014 evaluates correctly, is it maybe the ternary operator on line 11057 (the corresponding force one on line 11032 seems to be the other way around)?

Or is this the intender behaviour and there is something else I'm missing?