Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
erwincoumans
2017-08-30 19:41:15 -07:00
parent 8dfa76e924
commit ee30479a28
7 changed files with 105 additions and 19 deletions
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,
};