Expose motor drive torque reporting for motors in spherical joints in getJointStateMultiDof.

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1047,6 +1047,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
 				for (int i = 0; i < state->m_uDofSize; i++)
 				{
 					state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i];
+					state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_jointMotorForceMultiDof[info.m_uIndex + i];
 				}
 			}
 			else
@@ -1058,7 +1059,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
 			{
 				state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
 			}
-			state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
+			
 			return 1;
 		}
 	}

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6425,6 +6425,32 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 			}
 			for (int d = 0; d < mb->getLink(l).m_dofCount; d++)
 			{
+				serverCmd.m_sendActualStateArgs.m_jointMotorForce[totalDegreeOfFreedomU] = 0;
+
+				if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical)
+				{
+					btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(l).m_userPtr;
+					if (motor)
+					{
+						btScalar impulse = motor->getAppliedImpulse(d);
+						btScalar force = impulse / m_data->m_physicsDeltaTime;
+						serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
+					}
+				}
+				else
+				{
+					if (supportsJointMotor(mb, l))
+					{
+						btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
+
+						if (motor && m_data->m_physicsDeltaTime > btScalar(0))
+						{
+							btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime;
+							serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
+						}
+					}
+				}
+
 				serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
 			}
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -531,6 +531,7 @@ struct SendActualStateArgs
 	double m_jointReactionForces[6 * MAX_DEGREE_OF_FREEDOM];
 
 	double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
+	double m_jointMotorForceMultiDof[MAX_DEGREE_OF_FREEDOM];
 
 	double m_linkState[7 * MAX_NUM_LINKS];
 	double m_linkWorldVelocities[6 * MAX_NUM_LINKS];  //linear velocity and angular velocity in world space (x/y/z each).

examples/SharedMemory/SharedMemoryPublic.h

@@ -369,7 +369,7 @@ struct b3JointSensorState2
 	double m_jointPosition[4];
 	double m_jointVelocity[3];
 	double m_jointReactionForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
-	double m_jointMotorTorque;
+	double m_jointMotorTorqueMultiDof[3];
 	int m_qDofSize;
 	int m_uDofSize;
 };

examples/pybullet/examples/humanoidMotionCapture.py

@@ -14,10 +14,10 @@ useYUp = not useZUp
 if useYUp:
   p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
 
-from pdControllerExplicit import PDControllerExplicitMultiDof
+
 from pdControllerStable import PDControllerStableMultiDof
 
-explicitPD = PDControllerExplicitMultiDof(p)
+
 stablePD = PDControllerStableMultiDof(p)
 
 p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
@@ -26,7 +26,7 @@ import pybullet_data
 p.setTimeOut(10000)
 useMotionCapture=False
 useMotionCaptureReset=False#not useMotionCapture
-useExplicitPD = True
+
 
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 p.setPhysicsEngineParameter(numSolverIterations=30)
@@ -76,14 +76,8 @@ p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,s
 p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0])
 
 humanoid =  p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
-humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
-humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
-humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
 
 humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1])
-humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1])
-humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1])
-humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1])
 
 
 startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
@@ -96,10 +90,6 @@ startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-
 -4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
 
 p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
-p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1])
-p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1])
-p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1])
-
 
 
 index0=7
@@ -114,7 +104,6 @@ for j in range (p.getNumJoints(humanoid)):
     print("spherical position: ",targetPosition)
     print("spherical velocity: ",targetVel)
     p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
-		p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
   if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
     targetPosition=[startPose[index0]]
     targetVel = [startVel[index0]]
@@ -122,9 +111,6 @@ for j in range (p.getNumJoints(humanoid)):
     print("revolute:", targetPosition)
     print("revolute velocity:", targetVel)
     p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
-		p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
-	
-
 
 
 for j in range (p.getNumJoints(humanoid)):
@@ -134,13 +120,9 @@ for j in range (p.getNumJoints(humanoid)):
   if (jointType == p.JOINT_SPHERICAL):
     targetPosition=[0,0,0,1]
     p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0])
-		p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31])
-		p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1])
 
   if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
     p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0)
-		p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31)
-		p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10)
   
   #print(ji)
   print("joint[",j,"].type=",jointTypes[ji[2]])
@@ -199,7 +181,6 @@ p.getCameraImage(320,200)
 
 
 
-
 while (p.isConnected()):
 
   if useGUI:  
@@ -221,11 +202,6 @@ while (p.isConnected()):
     frameNext = frame
   
   frameFraction = frameReal - frame
-	#print("frameFraction=",frameFraction)
-	#print("frame=",frame)
-	#print("frameNext=", frameNext)
-	
-	#getQuaternionSlerp
   
   frameData = motion_dict['Frames'][frame]
   frameDataNext = motion_dict['Frames'][frameNext]
@@ -242,17 +218,41 @@ while (p.isConnected()):
   baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
   baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
   #pre-rotate to make z-up
+  
+  
   if (useZUp):
     y2zPos=[0,0,0.0]
     y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
     basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
     p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
   
-		y2zPos=[0,2,0.0]
+  
+  once=False
+  #print("chestRot=",chestRot)
+  p.setGravity(0,0,0)
+  
+  kp=kpMotor
+    
+    
+    
+    
+    
+    
+  
+  basePos1Start = [frameData[1],frameData[2],frameData[3]]
+  basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+  basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), 
+    basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), 
+    basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
+  baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
+  baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
+  baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
+  #pre-rotate to make z-up
+  if (useZUp):
+    y2zPos=[0,0,0.0]
     y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
     basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
-		p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
+    p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
-	
   
   chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
   chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
@@ -301,45 +301,12 @@ while (p.isConnected()):
   leftElbowRotEnd = [frameDataNext[43]]
   leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
   
-	if (0):#if (once):
-		p.resetJointStateMultiDof(humanoid,chest,chestRot)
-		p.resetJointStateMultiDof(humanoid,neck,neckRot)
-		p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
-		p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot)
-		p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot)
-		p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot)
-		p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot)
-		p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot)
-		p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot)
-		p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
-		p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
-		p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
   once=False
-	#print("chestRot=",chestRot)
+  
-	p.setGravity(0,0,-10)
+  
   
   kp=kpMotor
-	if (useExplicitPD):
-		jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
-		#[x,y,z] base position and [x,y,z,w] base orientation!
-		totalDofs =7 
-		for dof in jointDofCounts:
-			totalDofs += dof
   
-		jointIndicesAll = [
-								chest,
-								neck, 
-								rightHip,
-												rightKnee,
-								rightAnkle,
-								rightShoulder,
-												rightElbow,
-								leftHip,
-												leftKnee,
-								leftAnkle,
-								leftShoulder,
-												leftElbow
-		]
   basePos,baseOrn = p.getBasePositionAndOrientation(humanoid)
   pose = [ basePos[0],basePos[1],basePos[2],
                           baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
@@ -356,12 +323,27 @@ while (p.isConnected()):
                           leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
                           leftElbowRot[0] ]
                           
+  jointIndicesAll = [
+                chest,
+                neck, 
+                rightHip,
+                        rightKnee,
+                rightAnkle,
+                rightShoulder,
+                        rightElbow,
+                leftHip,
+                        leftKnee,
+                leftAnkle,
+                leftShoulder,
+                        leftElbow
+    ]
+  dofIndex=7
+  jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
+  totalDofs = sum(jointDofCounts)+7
+  print("total=",totalDofs)
   
-		#print("pose=")
+  useStablePD=False
-		#for po in pose:
+  if (useStablePD):
-		#	print(po)
-		
-		
     taus = stablePD.computePD(bodyUniqueId=humanoid, 
       jointIndices = jointIndicesAll,
       desiredPositions = pose,
@@ -371,61 +353,39 @@ while (p.isConnected()):
       maxForces = [maxForce]*totalDofs, 
       timeStep=timeStep)
       
-		taus3 = explicitPD.computePD(bodyUniqueId=humanoid3, 
-			jointIndices = jointIndicesAll,
-			desiredPositions = pose,
-			desiredVelocities = [0]*totalDofs,
-			kps = kpOrg,
-			kds = kdOrg,
-			maxForces = [maxForce*0.05]*totalDofs, 
-			timeStep=timeStep)
   
     #taus=[0]*43
-		dofIndex=7
+
     for index in range (len(jointIndicesAll)):
       jointIndex = jointIndicesAll[index]
       if jointDofCounts[index]==4:
-				
         p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
-				p.setJointMotorControlMultiDof(humanoid3,jointIndex,p.TORQUE_CONTROL,force=[taus3[dofIndex+0],taus3[dofIndex+1],taus3[dofIndex+2]])
-	
       if jointDofCounts[index]==1:
-				
         p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
-				p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]])
-
       dofIndex+=jointDofCounts[index]
-		
+  else:
-		#print("len(taus)=",len(taus))
+    p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
-		#print("taus=",taus)
+    p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
-		
+    p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
-		
+    p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce])
+    p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce])
-	p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
-	
-	kinematicHumanoid4 = True
-	if (kinematicHumanoid4):
-		p.resetJointStateMultiDof(humanoid4,chest,chestRot)
-		p.resetJointStateMultiDof(humanoid4,neck,neckRot)
-		p.resetJointStateMultiDof(humanoid4,rightHip,rightHipRot)
-		p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot)
-		p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot)
-		p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot)
-		p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot)
-		p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot)
-		p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot)
-		p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot)
-		p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot)
-		p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot)
   p.stepSimulation()
+  for j in range(p.getNumJoints(humanoid)):
+    jointState = p.getJointStateMultiDof(humanoid,j)
+    print("jointStateMultiDof[",j,"].pos=",jointState[0])
+    print("jointStateMultiDof[",j,"].vel=",jointState[1])
+    print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
+    
+  #j=4
+  #jointState = p.getJointStateMultiDof(humanoid,j)
+  #print("jointStateMultiDof[",j,"].pos=",jointState[0])
+  #print("jointStateMultiDof[",j,"].vel=",jointState[1])
+  #print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
   time.sleep(timeStep)

examples/pybullet/pybullet.c

@@ -4235,6 +4235,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 	PyObject* pyListJointState;
 	PyObject* pyListPosition;
 	PyObject* pyListVelocity;
+	PyObject* pyListJointMotorTorque;
 
 
 	struct b3JointSensorState2 sensorState;
@@ -4296,7 +4297,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 				int i = 0;
 				pyListPosition = PyTuple_New(sensorState.m_qDofSize);
 				pyListVelocity = PyTuple_New(sensorState.m_uDofSize);
-
+				pyListJointMotorTorque = PyTuple_New(sensorState.m_uDofSize);
 				PyTuple_SetItem(pyListJointState, 0, pyListPosition);
 				PyTuple_SetItem(pyListJointState, 1, pyListVelocity);
 
@@ -4310,6 +4311,9 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 				{
 					PyTuple_SetItem(pyListVelocity, i,
 						PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
+
+					PyTuple_SetItem(pyListJointMotorTorque, i, 
+						PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
 				}
 				
 				for (j = 0; j < forceTorqueSize; j++)
@@ -4320,8 +4324,8 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 
 				PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
 
-				PyTuple_SetItem(pyListJointState, 3,
+				PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
-					PyFloat_FromDouble(sensorState.m_jointMotorTorque));
+					
 
 				return pyListJointState;
 			}