add option to log joint torques (due to user applied torques and/or motor torques)

See quadruped.py for an example:
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES)
Thanks to JulianYG, in pull request https://github.com/bulletphysics/bullet3/pull/1273

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3657,6 +3657,22 @@ int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int ma
 	return 0;
 }
 
+int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_STATE_LOGGING);
+	if (command->m_type == CMD_STATE_LOGGING)
+	{
+	    command->m_updateFlags |= STATE_LOGGING_LOG_FLAGS;
+		command->m_stateLoggingArguments.m_logFlags = logFlags;
+	}
+	return 0;
+}
+
+
+
+
 int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -471,6 +471,7 @@ int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int l
 int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId);
 int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId);
 int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
+int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags);
 
 int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
 int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -844,13 +844,15 @@ struct GenericRobotStateLogger : public InternalStateLogger
     btAlignedObjectArray<int> m_bodyIdList;
     bool m_filterObjectUniqueId;
     int m_maxLogDof;
+	int m_logFlags;
 
-    GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof)
+    GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
     :m_loggingTimeStamp(0),
     m_logFileHandle(0),
     m_dynamicsWorld(dynamicsWorld),
     m_filterObjectUniqueId(false),
-	m_maxLogDof(maxLogDof)
+	m_maxLogDof(maxLogDof),
+	m_logFlags(logFlags)
     {
         m_loggingUniqueId = loggingUniqueId;
         m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
@@ -883,14 +885,26 @@ struct GenericRobotStateLogger : public InternalStateLogger
 			sprintf(jointName,"q%d",i);
 			structNames.push_back(jointName);
 		}
+
 		for (int i=0;i<m_maxLogDof;i++)
 		{
 			m_structTypes.append("f");
 			char jointName[256];
-			sprintf(jointName,"u%d",i);
+			sprintf(jointName,"v%d",i);
 			structNames.push_back(jointName);
 		}
-        
+
+		if (m_logFlags & STATE_LOG_JOINT_TORQUES)
+		{
+			for (int i=0;i<m_maxLogDof;i++)
+			{
+				m_structTypes.append("f");
+				char jointName[256];
+				sprintf(jointName,"u%d",i);
+				structNames.push_back(jointName);
+			}
+		}
+
         const char* fileNameC = fileName.c_str();
         
         m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
@@ -979,15 +993,49 @@ struct GenericRobotStateLogger : public InternalStateLogger
                 {
                     if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
                     {
-                        float u = mb->getJointVel(j);
+                        float v = mb->getJointVel(j);
-                        logData.m_values.push_back(u);
+                        logData.m_values.push_back(v);
                     }
                 }
                 for (int j = numDofs; j < m_maxLogDof; ++j)
                 {
-                    float u = 0.0;
+                    float v = 0.0;
-                    logData.m_values.push_back(u);
+                    logData.m_values.push_back(v);
                 }
+
+				
+				if (m_logFlags & STATE_LOG_JOINT_TORQUES)
+				{
+					for (int j = 0; j < numJoints; ++j)
+					{
+						if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
+						{
+							float jointTorque = 0;
+							if (m_logFlags & STATE_LOG_JOINT_MOTOR_TORQUES)
+							{
+								btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(j).m_userPtr;
+								if (motor)
+								{
+									jointTorque += motor->getAppliedImpulse(0)/timeStep;
+								}
+							}
+							if (m_logFlags & STATE_LOG_JOINT_USER_TORQUES)
+							{
+								if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
+								{
+									jointTorque += mb->getJointTorque(j);//these are the 'user' applied external torques
+								}
+							}
+							logData.m_values.push_back(jointTorque);
+						}
+
+					}
+					for (int j = numDofs; j < m_maxLogDof; ++j)
+					{
+						float u = 0.0;
+						logData.m_values.push_back(u);
+					}
+				}
                                 
                 //at the moment, appendMinitaurLogData will directly write to disk (potential delay)
                 //better to fill a huge memory buffer and once in a while write it to disk
@@ -999,7 +1047,6 @@ struct GenericRobotStateLogger : public InternalStateLogger
         }
     }
 };
-
 struct ContactPointsStateLogger : public InternalStateLogger
 {
 	int m_loggingTimeStamp;
@@ -2848,7 +2895,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							{
 								maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
 							}
-                            GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof);
+							
+							int logFlags = 0;
+							if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS)
+							{
+								logFlags = clientCmd.m_stateLoggingArguments.m_logFlags;
+							}
+                            GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof, logFlags);
                             
                             if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
                             {

examples/SharedMemory/SharedMemoryCommands.h

@@ -760,7 +760,8 @@ enum eStateLoggingEnums
 	STATE_LOGGING_FILTER_LINK_INDEX_B=32,
 	STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A=64,
 	STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B=128,
-	STATE_LOGGING_FILTER_DEVICE_TYPE=256
+	STATE_LOGGING_FILTER_DEVICE_TYPE=256,
+	STATE_LOGGING_LOG_FLAGS=512
 };
 
 struct VRCameraState
@@ -786,6 +787,7 @@ struct StateLoggingRequest
 	int m_bodyUniqueIdA; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A flag is set
 	int m_bodyUniqueIdB; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B flag is set
 	int m_deviceFilterType; //user to select (filter) which VR devices to log
+	int m_logFlags;
 };
 
 struct StateLoggingResultArgs

examples/SharedMemory/SharedMemoryPublic.h

@@ -607,6 +607,12 @@ enum eUrdfCollisionFlags
 	GEOM_FORCE_CONCAVE_TRIMESH=1,
 };
 
+enum eStateLoggingFlags
+{
+	STATE_LOG_JOINT_MOTOR_TORQUES=1,
+	STATE_LOG_JOINT_USER_TORQUES=2,
+	STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
+};
 
 
 

examples/pybullet/examples/quadruped.py

@@ -19,7 +19,7 @@ if (physId<0):
 #p.resetSimulation()
 p.loadURDF("plane.urdf",0,0,0)
 p.setPhysicsEngineParameter(numSolverIterations=50)
-
+p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES)
 p.setTimeOut(4)
 
 p.setGravity(0,0,0)
@@ -156,7 +156,7 @@ t_end = t + 100
 i=0
 ref_time = time.time()
 
-while t < t_end:
+for aa in range (100):
 	if (useRealTime):
 		t = time.time()-ref_time
 	else:

examples/pybullet/pybullet.c

@@ -3573,12 +3573,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
 	int linkIndexA = -2;
 	int linkIndexB = -2;
 	int deviceTypeFilter = -1;
+	int logFlags = -1;
 
-	static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "physicsClientId", NULL};
+	static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "logFlags", "physicsClientId", NULL};
 	int physicsClientId = 0;
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiii", kwlist,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiiii", kwlist,
-									 &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &physicsClientId))
+									 &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &logFlags, &physicsClientId))
 		return NULL;
 
 	sm = getPhysicsClient(physicsClientId);
@@ -3636,6 +3637,11 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
 			b3StateLoggingSetDeviceTypeFilter(commandHandle,deviceTypeFilter);
 		}
 
+		if (logFlags >0)
+		{
+			b3StateLoggingSetLogFlags(commandHandle, logFlags);
+		}
+
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 		statusType = b3GetStatusType(statusHandle);
 		if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
@@ -7472,9 +7478,11 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
 
 	PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH);
-
+	
-
+	PyModule_AddIntConstant(m, "STATE_LOG_JOINT_MOTOR_TORQUES", STATE_LOG_JOINT_MOTOR_TORQUES);
-
+	PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);
+	PyModule_AddIntConstant(m, "STATE_LOG_JOINT_TORQUES", STATE_LOG_JOINT_USER_TORQUES+STATE_LOG_JOINT_MOTOR_TORQUES);
+		
 	SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
 	Py_INCREF(SpamError);
 	PyModule_AddObject(m, "error", SpamError);