From ee30479a283e1f234d38cd3133f52f6334846f9c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 30 Aug 2017 19:41:15 -0700 Subject: [PATCH] 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 | 16 ++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 73 ++++++++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 4 +- examples/SharedMemory/SharedMemoryPublic.h | 6 ++ examples/pybullet/examples/quadruped.py | 4 +- examples/pybullet/pybullet.c | 20 +++-- 7 files changed, 105 insertions(+), 19 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d32f4f0da..46c4ae0be 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 1f340f217..03869e314 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f1ff18d6e..2e944481f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -844,13 +844,15 @@ struct GenericRobotStateLogger : public InternalStateLogger btAlignedObjectArray 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;igetLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1) { - float u = mb->getJointVel(j); - logData.m_values.push_back(u); + float v = mb->getJointVel(j); + logData.m_values.push_back(v); } } for (int j = numDofs; j < m_maxLogDof; ++j) { - float u = 0.0; - logData.m_values.push_back(u); + float v = 0.0; + 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)) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index a11712ba0..357314740 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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 diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 467fd9852..13b8f18a5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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, +}; diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 97c5c3889..9495a7acc 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/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: diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index af356f4bb..e1df46636 100644 --- a/examples/pybullet/pybullet.c +++ b/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, - &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiiii", kwlist, + &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);