When I use my control algorithm to calculate motor torque and provide it to motor by using setJointMotorControl2,
I should get the same applied torque from getJointState after stepping the next simulation but it is not.The code seems like
while 1:
torque = controlAlgorithm(args)
setJointMotorControl2(robotId, jointId, TORQUE_CONTROL, force = torque)
stepSimulation()
appliedTorque = getJointState(robotId, jointId)[3]
torque == appliedTorque(False)
But when I provide constant torque like torque = 1 and leave the other code unchanged, appliedTorque equals torque.
What's the problem?