AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1288 from erwincoumans/master

Browse Source 
add option to log joint torques (due to user applied torques and/or m…
This commit is contained in:
erwincoumans 2017-08-30 20:45:56 -07:00 committed by GitHub
commit 29271c53eb
7 changed files with 105 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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);

73
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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);
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))
{

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -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

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -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,
};

4
examples/pybullet/examples/quadruped.py
View File

@ -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:

20
examples/pybullet/pybullet.c
View File

@ -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);