Improve ChromeTraceUtil logging, allow filename to be specified.
Expose this ChromeTraceUtil logging to C-API: start/statelogging and submitProfileTiming pybullet.submitProfileTiming, and STATE_LOGGING_PROFILE_TIMINGS used in startStateLogging added example for Python profileTiming.py and C++ b3RobotSimulatorClientAPI::submitProfileTiming
This commit is contained in:
@@ -260,7 +260,7 @@ void MyKeyboardCallback(int key, int state)
|
|||||||
|
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
b3ChromeUtilsStopTimingsAndWriteJsonFile();
|
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
|
||||||
}
|
}
|
||||||
#endif //BT_NO_PROFILE
|
#endif //BT_NO_PROFILE
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -912,6 +912,11 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
|
|||||||
|
|
||||||
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
|
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
|
||||||
{
|
{
|
||||||
|
if (!isConnected())
|
||||||
|
{
|
||||||
|
b3Warning("Not connected");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
int loggingUniqueId = -1;
|
int loggingUniqueId = -1;
|
||||||
b3SharedMemoryCommandHandle commandHandle;
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
||||||
@@ -942,6 +947,12 @@ int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType,
|
|||||||
|
|
||||||
void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
|
void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
|
||||||
{
|
{
|
||||||
|
if (!isConnected())
|
||||||
|
{
|
||||||
|
b3Warning("Not connected");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle commandHandle;
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
||||||
@@ -951,6 +962,12 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
|
|||||||
|
|
||||||
void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos)
|
void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos)
|
||||||
{
|
{
|
||||||
|
if (!isConnected())
|
||||||
|
{
|
||||||
|
b3Warning("Not connected");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
|
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
|
||||||
if (commandHandle)
|
if (commandHandle)
|
||||||
{
|
{
|
||||||
@@ -963,3 +980,19 @@ void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance
|
|||||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
|
||||||
|
{
|
||||||
|
if (!isConnected())
|
||||||
|
{
|
||||||
|
b3Warning("Not connected");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
|
||||||
|
if (durationInMicroSeconds>=0)
|
||||||
|
{
|
||||||
|
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
|
||||||
|
}
|
||||||
|
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||||
|
}
|
||||||
@@ -210,6 +210,9 @@ public:
|
|||||||
|
|
||||||
void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
|
void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
|
||||||
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
|
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
|
||||||
|
|
||||||
|
void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
|
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
|
||||||
|
|||||||
@@ -2695,6 +2695,41 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name)
|
||||||
|
{
|
||||||
|
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||||
|
b3Assert(cl);
|
||||||
|
b3Assert(cl->canSubmitCommand());
|
||||||
|
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||||
|
b3Assert(command);
|
||||||
|
|
||||||
|
int len = strlen(name);
|
||||||
|
if (len>=0 && len < (MAX_FILENAME_LENGTH+1))
|
||||||
|
{
|
||||||
|
command->m_type = CMD_PROFILE_TIMING;
|
||||||
|
strcpy(command->m_profile.m_name,name);
|
||||||
|
command->m_profile.m_name[len]=0;
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
const char* invalid = "InvalidProfileTimingName";
|
||||||
|
int len = strlen(invalid);
|
||||||
|
strcpy(command->m_profile.m_name,invalid);
|
||||||
|
command->m_profile.m_name[len] = 0;
|
||||||
|
}
|
||||||
|
command->m_profile.m_durationInMicroSeconds = 0;
|
||||||
|
return (b3SharedMemoryCommandHandle)command;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration)
|
||||||
|
{
|
||||||
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
b3Assert(command);
|
||||||
|
b3Assert(command->m_type == CMD_PROFILE_TIMING);
|
||||||
|
if (command->m_type == CMD_PROFILE_TIMING)
|
||||||
|
{
|
||||||
|
command->m_profile.m_durationInMicroSeconds = duration;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
|
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
|
||||||
{
|
{
|
||||||
@@ -2841,7 +2876,6 @@ int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle,
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
|
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
|
||||||
{
|
{
|
||||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
|||||||
@@ -392,6 +392,8 @@ int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
|||||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
|
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
|
||||||
|
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
|
||||||
|
void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
|
||||||
|
|
||||||
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
|
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
|
||||||
double b3GetTimeOut(b3PhysicsClientHandle physClient);
|
double b3GetTimeOut(b3PhysicsClientHandle physClient);
|
||||||
|
|||||||
@@ -13,7 +13,9 @@
|
|||||||
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
|
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
||||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||||
#include "LinearMath/btHashMap.h"
|
#include "Bullet3Common/b3HashMap.h"
|
||||||
|
#include "../Utils/ChromeTraceUtil.h"
|
||||||
|
|
||||||
#include "BulletInverseDynamics/MultiBodyTree.hpp"
|
#include "BulletInverseDynamics/MultiBodyTree.hpp"
|
||||||
#include "IKTrajectoryHelper.h"
|
#include "IKTrajectoryHelper.h"
|
||||||
#include "btBulletDynamicsCommon.h"
|
#include "btBulletDynamicsCommon.h"
|
||||||
@@ -28,7 +30,7 @@
|
|||||||
#include "SharedMemoryCommands.h"
|
#include "SharedMemoryCommands.h"
|
||||||
#include "LinearMath/btRandom.h"
|
#include "LinearMath/btRandom.h"
|
||||||
#include "Bullet3Common/b3ResizablePool.h"
|
#include "Bullet3Common/b3ResizablePool.h"
|
||||||
|
#include "../Utils/b3Clock.h"
|
||||||
#ifdef B3_ENABLE_TINY_AUDIO
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
#include "../TinyAudio/b3SoundEngine.h"
|
#include "../TinyAudio/b3SoundEngine.h"
|
||||||
#endif
|
#endif
|
||||||
@@ -132,7 +134,7 @@ struct InteralBodyData
|
|||||||
btTransform m_rootLocalInertialFrame;
|
btTransform m_rootLocalInertialFrame;
|
||||||
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
|
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
|
||||||
#ifdef B3_ENABLE_TINY_AUDIO
|
#ifdef B3_ENABLE_TINY_AUDIO
|
||||||
btHashMap<btHashInt, SDFAudioSource> m_audioSources;
|
b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
|
||||||
#endif //B3_ENABLE_TINY_AUDIO
|
#endif //B3_ENABLE_TINY_AUDIO
|
||||||
|
|
||||||
InteralBodyData()
|
InteralBodyData()
|
||||||
@@ -1158,11 +1160,11 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
btScalar m_physicsDeltaTime;
|
btScalar m_physicsDeltaTime;
|
||||||
btScalar m_numSimulationSubSteps;
|
btScalar m_numSimulationSubSteps;
|
||||||
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
||||||
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
||||||
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
|
b3HashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
|
||||||
|
|
||||||
int m_userConstraintUIDGenerator;
|
int m_userConstraintUIDGenerator;
|
||||||
btHashMap<btHashInt, InteralUserConstraintData> m_userConstraints;
|
b3HashMap<btHashInt, InteralUserConstraintData> m_userConstraints;
|
||||||
|
|
||||||
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
|
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
|
||||||
|
|
||||||
@@ -1198,6 +1200,8 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
|
|
||||||
btAlignedObjectArray<InternalStateLogger*> m_stateLoggers;
|
btAlignedObjectArray<InternalStateLogger*> m_stateLoggers;
|
||||||
int m_stateLoggersUniqueId;
|
int m_stateLoggersUniqueId;
|
||||||
|
int m_profileTimingLoggingUid;
|
||||||
|
std::string m_profileTimingFileName;
|
||||||
|
|
||||||
struct GUIHelperInterface* m_guiHelper;
|
struct GUIHelperInterface* m_guiHelper;
|
||||||
int m_sharedMemoryKey;
|
int m_sharedMemoryKey;
|
||||||
@@ -1219,6 +1223,8 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
b3SoundEngine m_soundEngine;
|
b3SoundEngine m_soundEngine;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
b3HashMap<b3HashString, char*> m_profileEvents;
|
||||||
|
|
||||||
PhysicsServerCommandProcessorInternalData()
|
PhysicsServerCommandProcessorInternalData()
|
||||||
:
|
:
|
||||||
m_allowRealTimeSimulation(false),
|
m_allowRealTimeSimulation(false),
|
||||||
@@ -1247,6 +1253,7 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
m_dynamicsWorld(0),
|
m_dynamicsWorld(0),
|
||||||
m_remoteDebugDrawer(0),
|
m_remoteDebugDrawer(0),
|
||||||
m_stateLoggersUniqueId(0),
|
m_stateLoggersUniqueId(0),
|
||||||
|
m_profileTimingLoggingUid(-1),
|
||||||
m_guiHelper(0),
|
m_guiHelper(0),
|
||||||
m_sharedMemoryKey(SHARED_MEMORY_KEY),
|
m_sharedMemoryKey(SHARED_MEMORY_KEY),
|
||||||
m_verboseOutput(false),
|
m_verboseOutput(false),
|
||||||
@@ -1372,7 +1379,11 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
|
|||||||
delete m_data->m_commandLogger;
|
delete m_data->m_commandLogger;
|
||||||
m_data->m_commandLogger = 0;
|
m_data->m_commandLogger = 0;
|
||||||
}
|
}
|
||||||
|
for (int i=0;i<m_data->m_profileEvents.size();i++)
|
||||||
|
{
|
||||||
|
char* event = *m_data->m_profileEvents.getAtIndex(i);
|
||||||
|
delete[] event;
|
||||||
|
}
|
||||||
delete m_data;
|
delete m_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2210,7 +2221,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
|
|||||||
|
|
||||||
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
|
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
|
||||||
{
|
{
|
||||||
BT_PROFILE("processCommand");
|
// BT_PROFILE("processCommand");
|
||||||
|
|
||||||
bool hasStatus = false;
|
bool hasStatus = false;
|
||||||
|
|
||||||
@@ -2278,7 +2289,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
|
|
||||||
if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
|
if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
|
||||||
{
|
{
|
||||||
|
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS)
|
||||||
|
{
|
||||||
|
if (m_data->m_profileTimingLoggingUid<0)
|
||||||
|
{
|
||||||
|
b3ChromeUtilsStartTimings();
|
||||||
|
m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName;
|
||||||
|
int loggerUid = m_data->m_stateLoggersUniqueId++;
|
||||||
|
serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
|
||||||
|
serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
|
||||||
|
m_data->m_profileTimingLoggingUid = loggerUid;
|
||||||
|
}
|
||||||
|
}
|
||||||
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4)
|
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4)
|
||||||
{
|
{
|
||||||
//if (clientCmd.m_stateLoggingArguments.m_fileName)
|
//if (clientCmd.m_stateLoggingArguments.m_fileName)
|
||||||
@@ -2416,14 +2438,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
}
|
}
|
||||||
if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0)
|
if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0)
|
||||||
{
|
{
|
||||||
serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
|
if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid)
|
||||||
for (int i=0;i<m_data->m_stateLoggers.size();i++)
|
|
||||||
{
|
{
|
||||||
if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId)
|
b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str());
|
||||||
|
m_data->m_profileTimingLoggingUid = -1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
|
||||||
|
for (int i=0;i<m_data->m_stateLoggers.size();i++)
|
||||||
{
|
{
|
||||||
m_data->m_stateLoggers[i]->stop();
|
if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId)
|
||||||
delete m_data->m_stateLoggers[i];
|
{
|
||||||
m_data->m_stateLoggers.removeAtIndex(i);
|
m_data->m_stateLoggers[i]->stop();
|
||||||
|
delete m_data->m_stateLoggers[i];
|
||||||
|
m_data->m_stateLoggers.removeAtIndex(i);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2458,7 +2488,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
}
|
}
|
||||||
case CMD_REQUEST_VR_EVENTS_DATA:
|
case CMD_REQUEST_VR_EVENTS_DATA:
|
||||||
{
|
{
|
||||||
BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA");
|
//BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA");
|
||||||
serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
|
serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
|
||||||
|
|
||||||
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
|
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
|
||||||
@@ -2486,7 +2516,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
|
|
||||||
case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
|
case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
|
||||||
{
|
{
|
||||||
BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA");
|
//BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA");
|
||||||
|
|
||||||
serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size();
|
serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size();
|
||||||
if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS)
|
if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS)
|
||||||
@@ -3320,6 +3350,56 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
case CMD_PROFILE_TIMING:
|
||||||
|
{
|
||||||
|
{
|
||||||
|
B3_PROFILE("custom");//clientCmd.m_profile.m_name);
|
||||||
|
{
|
||||||
|
B3_PROFILE("event");//clientCmd.m_profile.m_name);
|
||||||
|
char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name];
|
||||||
|
char* eventName = 0;
|
||||||
|
if (eventNamePtr)
|
||||||
|
{
|
||||||
|
B3_PROFILE("reuse");
|
||||||
|
eventName = *eventNamePtr;
|
||||||
|
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
B3_PROFILE("alloc");
|
||||||
|
int len = strlen(clientCmd.m_profile.m_name);
|
||||||
|
eventName = new char[len+1];
|
||||||
|
strcpy(eventName,clientCmd.m_profile.m_name);
|
||||||
|
eventName[len] = 0;
|
||||||
|
m_data->m_profileEvents.insert(eventName,eventName);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
{
|
||||||
|
{
|
||||||
|
B3_PROFILE("with");//clientCmd.m_profile.m_name);
|
||||||
|
{
|
||||||
|
B3_PROFILE("some");//clientCmd.m_profile.m_name);
|
||||||
|
{
|
||||||
|
B3_PROFILE("deep");//clientCmd.m_profile.m_name);
|
||||||
|
{
|
||||||
|
B3_PROFILE("level");//clientCmd.m_profile.m_name);
|
||||||
|
{
|
||||||
|
B3_PROFILE(eventName);
|
||||||
|
b3Clock::usleep(clientCmd.m_profile.m_durationInMicroSeconds);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||||
|
hasStatus = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case CMD_SEND_DESIRED_STATE:
|
case CMD_SEND_DESIRED_STATE:
|
||||||
{
|
{
|
||||||
BT_PROFILE("CMD_SEND_DESIRED_STATE");
|
BT_PROFILE("CMD_SEND_DESIRED_STATE");
|
||||||
|
|||||||
@@ -252,7 +252,7 @@ void PhysicsServerSharedMemory::processClientCommands()
|
|||||||
if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
|
if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
|
||||||
{
|
{
|
||||||
|
|
||||||
BT_PROFILE("processClientCommand");
|
//BT_PROFILE("processClientCommand");
|
||||||
|
|
||||||
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
|
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
|
||||||
btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1);
|
btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1);
|
||||||
|
|||||||
@@ -459,6 +459,14 @@ struct b3ObjectArgs
|
|||||||
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
|
int m_userConstraintUniqueIds[MAX_SDF_BODIES];
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct b3Profile
|
||||||
|
{
|
||||||
|
char m_name[MAX_FILENAME_LENGTH];
|
||||||
|
int m_durationInMicroSeconds;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct SdfLoadedArgs
|
struct SdfLoadedArgs
|
||||||
{
|
{
|
||||||
int m_numBodies;
|
int m_numBodies;
|
||||||
@@ -758,6 +766,7 @@ struct SharedMemoryCommand
|
|||||||
struct StateLoggingRequest m_stateLoggingArguments;
|
struct StateLoggingRequest m_stateLoggingArguments;
|
||||||
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
|
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
|
||||||
struct b3ObjectArgs m_removeObjectArgs;
|
struct b3ObjectArgs m_removeObjectArgs;
|
||||||
|
struct b3Profile m_profile;
|
||||||
|
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -57,6 +57,7 @@ enum EnumSharedMemoryClientCommand
|
|||||||
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA,
|
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA,
|
||||||
CMD_REMOVE_BODY,
|
CMD_REMOVE_BODY,
|
||||||
CMD_RESET_DYNAMIC_INFO,
|
CMD_RESET_DYNAMIC_INFO,
|
||||||
|
CMD_PROFILE_TIMING,
|
||||||
//don't go beyond this command!
|
//don't go beyond this command!
|
||||||
CMD_MAX_CLIENT_COMMANDS,
|
CMD_MAX_CLIENT_COMMANDS,
|
||||||
|
|
||||||
@@ -388,6 +389,7 @@ enum b3StateLoggingType
|
|||||||
STATE_LOGGING_VIDEO_MP4 = 3,
|
STATE_LOGGING_VIDEO_MP4 = 3,
|
||||||
STATE_LOGGING_COMMANDS = 4,
|
STATE_LOGGING_COMMANDS = 4,
|
||||||
STATE_LOGGING_CONTACT_POINTS = 5,
|
STATE_LOGGING_CONTACT_POINTS = 5,
|
||||||
|
STATE_LOGGING_PROFILE_TIMINGS = 6,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -355,7 +355,7 @@ void MyKeyboardCallback(int key, int state)
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
b3ChromeUtilsStopTimingsAndWriteJsonFile();
|
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (sExample)
|
if (sExample)
|
||||||
@@ -2342,7 +2342,7 @@ int main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (args.CheckCmdLineFlag("tracing"))
|
if (args.CheckCmdLineFlag("tracing"))
|
||||||
{
|
{
|
||||||
b3ChromeUtilsStopTimingsAndWriteJsonFile();
|
b3ChromeUtilsStopTimingsAndWriteJsonFile("timings");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -239,7 +239,7 @@ void b3ChromeUtilsStartTimings()
|
|||||||
btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
|
btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3ChromeUtilsStopTimingsAndWriteJsonFile()
|
void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix)
|
||||||
{
|
{
|
||||||
b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
|
b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
|
||||||
b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
|
b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
|
||||||
@@ -248,7 +248,7 @@ void b3ChromeUtilsStopTimingsAndWriteJsonFile()
|
|||||||
btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
|
btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
|
||||||
char fileName[1024];
|
char fileName[1024];
|
||||||
static int fileCounter = 0;
|
static int fileCounter = 0;
|
||||||
sprintf(fileName,"timings_%d.json",fileCounter++);
|
sprintf(fileName,"%s_%d.json",fileNamePrefix, fileCounter++);
|
||||||
gTimingFile = fopen(fileName,"w");
|
gTimingFile = fopen(fileName,"w");
|
||||||
fprintf(gTimingFile,"{\"traceEvents\":[\n");
|
fprintf(gTimingFile,"{\"traceEvents\":[\n");
|
||||||
//dump the content to file
|
//dump the content to file
|
||||||
|
|||||||
@@ -3,7 +3,7 @@
|
|||||||
#define B3_CHROME_TRACE_UTIL_H
|
#define B3_CHROME_TRACE_UTIL_H
|
||||||
|
|
||||||
void b3ChromeUtilsStartTimings();
|
void b3ChromeUtilsStartTimings();
|
||||||
void b3ChromeUtilsStopTimingsAndWriteJsonFile();
|
void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix);
|
||||||
void b3ChromeUtilsEnableProfiling();
|
void b3ChromeUtilsEnableProfiling();
|
||||||
|
|
||||||
#endif//B3_CHROME_TRACE_UTIL_H
|
#endif//B3_CHROME_TRACE_UTIL_H
|
||||||
12
examples/pybullet/examples/profileTiming.py
Normal file
12
examples/pybullet/examples/profileTiming.py
Normal file
@@ -0,0 +1,12 @@
|
|||||||
|
import pybullet as p
|
||||||
|
import time
|
||||||
|
|
||||||
|
p.connect(p.GUI)
|
||||||
|
|
||||||
|
t = time.time()+0.1
|
||||||
|
|
||||||
|
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
|
||||||
|
while (time.time()<t):
|
||||||
|
p.submitProfileTiming("pythontest")
|
||||||
|
p.stopStateLogging(logId)
|
||||||
|
|
||||||
@@ -2879,6 +2879,42 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
|
|||||||
return Py_None;
|
return Py_None;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static PyObject* pybullet_submitProfileTiming(PyObject* self, PyObject* args, PyObject* keywds)
|
||||||
|
{
|
||||||
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
|
int statusType;
|
||||||
|
char* eventName = 0;
|
||||||
|
int duractionInMicroSeconds=-1;
|
||||||
|
|
||||||
|
b3PhysicsClientHandle sm = 0;
|
||||||
|
static char* kwlist[] = {"eventName ", "duraction", "physicsClientId", NULL};
|
||||||
|
int physicsClientId = 0;
|
||||||
|
|
||||||
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist,
|
||||||
|
&eventName, &duractionInMicroSeconds, &physicsClientId))
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
sm = getPhysicsClient(physicsClientId);
|
||||||
|
if (sm == 0)
|
||||||
|
{
|
||||||
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
if (eventName)
|
||||||
|
{
|
||||||
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
|
commandHandle = b3ProfileTimingCommandInit(sm, eventName);
|
||||||
|
if (duractionInMicroSeconds>=0)
|
||||||
|
{
|
||||||
|
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, duractionInMicroSeconds);
|
||||||
|
}
|
||||||
|
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||||
|
}
|
||||||
|
Py_INCREF(Py_None);
|
||||||
|
return Py_None;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject* keywds)
|
static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject* keywds)
|
||||||
{
|
{
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
@@ -5691,6 +5727,10 @@ static PyMethodDef SpamMethods[] = {
|
|||||||
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
|
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
|
||||||
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
|
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
|
||||||
|
|
||||||
|
{"submitProfileTiming", (PyCFunction)pybullet_submitProfileTiming, METH_VARARGS | METH_KEYWORDS,
|
||||||
|
"Add a custom profile timing that will be visible in performance profile recordings on the physics server."
|
||||||
|
"On the physics server (in GUI and VR mode) you can press 'p' to start and/or stop profile recordings" },
|
||||||
|
|
||||||
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
|
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Set the timeOut in seconds, used for most of the API calls."},
|
"Set the timeOut in seconds, used for most of the API calls."},
|
||||||
// todo(erwincoumans)
|
// todo(erwincoumans)
|
||||||
@@ -5821,6 +5861,7 @@ initpybullet(void)
|
|||||||
PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS);
|
PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS);
|
||||||
PyModule_AddIntConstant(m, "STATE_LOGGING_VIDEO_MP4", STATE_LOGGING_VIDEO_MP4);
|
PyModule_AddIntConstant(m, "STATE_LOGGING_VIDEO_MP4", STATE_LOGGING_VIDEO_MP4);
|
||||||
PyModule_AddIntConstant(m, "STATE_LOGGING_CONTACT_POINTS", STATE_LOGGING_CONTACT_POINTS);
|
PyModule_AddIntConstant(m, "STATE_LOGGING_CONTACT_POINTS", STATE_LOGGING_CONTACT_POINTS);
|
||||||
|
PyModule_AddIntConstant(m, "STATE_LOGGING_PROFILE_TIMINGS", STATE_LOGGING_PROFILE_TIMINGS);
|
||||||
|
|
||||||
PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI);
|
PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI);
|
||||||
PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS);
|
PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS);
|
||||||
|
|||||||
@@ -399,6 +399,18 @@ protected:
|
|||||||
return &m_valueArray[index];
|
return &m_valueArray[index];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Key getKeyAtIndex(int index)
|
||||||
|
{
|
||||||
|
b3Assert(index < m_keyArray.size());
|
||||||
|
return m_keyArray[index];
|
||||||
|
}
|
||||||
|
|
||||||
|
const Key getKeyAtIndex(int index) const
|
||||||
|
{
|
||||||
|
b3Assert(index < m_keyArray.size());
|
||||||
|
return m_keyArray[index];
|
||||||
|
}
|
||||||
|
|
||||||
Value* operator[](const Key& key) {
|
Value* operator[](const Key& key) {
|
||||||
return find(key);
|
return find(key);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -374,7 +374,7 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
|
|||||||
|
|
||||||
void btDiscreteDynamicsWorld::synchronizeMotionStates()
|
void btDiscreteDynamicsWorld::synchronizeMotionStates()
|
||||||
{
|
{
|
||||||
BT_PROFILE("synchronizeMotionStates");
|
// BT_PROFILE("synchronizeMotionStates");
|
||||||
if (m_synchronizeAllMotionStates)
|
if (m_synchronizeAllMotionStates)
|
||||||
{
|
{
|
||||||
//iterate over all collision objects
|
//iterate over all collision objects
|
||||||
@@ -402,7 +402,6 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
|
|||||||
{
|
{
|
||||||
startProfiling(timeStep);
|
startProfiling(timeStep);
|
||||||
|
|
||||||
BT_PROFILE("stepSimulation");
|
|
||||||
|
|
||||||
int numSimulationSubSteps = 0;
|
int numSimulationSubSteps = 0;
|
||||||
|
|
||||||
|
|||||||
@@ -917,7 +917,7 @@ void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
|
|||||||
void btMultiBodyDynamicsWorld::clearMultiBodyForces()
|
void btMultiBodyDynamicsWorld::clearMultiBodyForces()
|
||||||
{
|
{
|
||||||
{
|
{
|
||||||
BT_PROFILE("clearMultiBodyForces");
|
// BT_PROFILE("clearMultiBodyForces");
|
||||||
for (int i=0;i<this->m_multiBodies.size();i++)
|
for (int i=0;i<this->m_multiBodies.size();i++)
|
||||||
{
|
{
|
||||||
btMultiBody* bod = m_multiBodies[i];
|
btMultiBody* bod = m_multiBodies[i];
|
||||||
|
|||||||
Reference in New Issue
Block a user