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

Merge pull request #964 from erwincoumans/master

Browse Source 
initial implementation of state logging
This commit is contained in:
erwincoumans
2017-02-17 15:39:46 -08:00
committed by GitHub
parent 835dea5c3f 8af62239b0
commit 6784cff23a
20 changed files with 620 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -317,6 +317,8 @@ SET(BulletExampleBrowser_SRCS
../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.h
../Importers/ImportURDFDemo/UrdfParser.cpp
../Utils/RobotLoggingUtil.cpp
../Utils/RobotLoggingUtil.h
../Importers/ImportURDFDemo/urdfStringSplit.cpp
../Importers/ImportURDFDemo/urdfStringSplit.h
../Importers/ImportURDFDemo/BulletUrdfImporter.cpp

3
examples/ExampleBrowser/premake4.lua
View File

@@ -97,6 +97,8 @@ project "App_BulletExampleBrowser"
"../BasicDemo/BasicExample.*",
"../Tutorial/*",
"../ExtendedTutorials/*",
"../Utils/RobotLoggingUtil.cpp",
"../Utils/RobotLoggingUtil.h",
"../Evolution/NN3DWalkers.cpp",
"../Evolution/NN3DWalkers.h",
"../Collision/*",
@@ -193,6 +195,7 @@ project "BulletExampleBrowserLib"
"OpenGLGuiHelper.cpp",
"OpenGLExampleBrowser.cpp",
"../Utils/b3Clock.cpp",
"../Utils/b3Clock.h",
"../Utils/ChromeTraceUtil.cpp",
"../Utils/ChromeTraceUtil.h",
"*.h",

2
examples/SharedMemory/CMakeLists.txt
View File

@@ -61,6 +61,8 @@ SET(SharedMemory_SRCS
../Importers/ImportMJCFDemo/BulletMJCFImporter.h
../Utils/b3ResourcePath.cpp
../Utils/b3Clock.cpp
../Utils/RobotLoggingUtil.cpp
../Utils/RobotLoggingUtil.h
../Utils/ChromeTraceUtil.cpp
../Utils/ChromeTraceUtil.h
../Importers/ImportURDFDemo/URDFImporterInterface.h

78
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2467,3 +2467,81 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o
return 0;
}
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_STATE_LOGGING;
command->m_updateFlags = 0;
command->m_stateLoggingArguments.m_numBodyUniqueIds = 0;
return (b3SharedMemoryCommandHandle)command;
}
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName)
{
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_START_LOG;
int len = strlen(fileName);
if (len < MAX_FILENAME_LENGTH)
{
strcpy(command->m_stateLoggingArguments.m_fileName, fileName);
}
else
{
command->m_stateLoggingArguments.m_fileName[0] = 0;
}
command->m_stateLoggingArguments.m_logType = loggingType;
}
return 0;
}
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_STATE_LOGGING_START_COMPLETED);
if (status && status->m_type == CMD_STATE_LOGGING_START_COMPLETED)
{
return status->m_stateLoggingResultArgs.m_loggingUniqueId;
}
return -1;
}
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
{
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_FILTER_OBJECT_UNIQUE_ID;
if (command->m_stateLoggingArguments.m_numBodyUniqueIds < MAX_SDF_BODIES)
{
command->m_stateLoggingArguments.m_bodyUniqueIds[command->m_stateLoggingArguments.m_numBodyUniqueIds++] = objectUniqueId;
}
}
return 0;
}
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
{
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_STOP_LOG;
command->m_stateLoggingArguments.m_loggingUniqueId = loggingUid;
}
return 0;
}

7
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -343,8 +343,11 @@ int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double
int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]);
int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
#ifdef __cplusplus
}

14
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -899,6 +899,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_STATE_LOGGING_START_COMPLETED:
{
break;
};
case CMD_STATE_LOGGING_COMPLETED:
{
break;
};
case CMD_STATE_LOGGING_FAILED:
{
b3Warning("State Logging failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);

251
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -17,7 +17,7 @@
#include "BulletInverseDynamics/MultiBodyTree.hpp"
#include "IKTrajectoryHelper.h"
#include "btBulletDynamicsCommon.h"
#include "../Utils/RobotLoggingUtil.h"
#include "LinearMath/btTransform.h"
#include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
@@ -420,6 +420,142 @@ struct MyOverlapFilterCallback : public btOverlapFilterCallback
};
struct InternalStateLogger
{
int m_loggingUniqueId;
int m_loggingType;
InternalStateLogger()
:m_loggingUniqueId(0),
m_loggingType(0)
{
}
virtual void stop() = 0;
virtual void logState(btScalar timeStamp)=0;
};
struct MinitaurStateLogger : public InternalStateLogger
{
int m_loggingTimeStamp;
std::string m_fileName;
int m_minitaurBodyUniqueId;
FILE* m_logFileHandle;
std::string m_structTypes;
btMultiBody* m_minitaurMultiBody;
btAlignedObjectArray<int> m_motorIdList;
MinitaurStateLogger(int loggingUniqueId, std::string fileName, btMultiBody* minitaurMultiBody, btAlignedObjectArray<int>& motorIdList)
:m_loggingTimeStamp(0),
m_logFileHandle(0),
m_minitaurMultiBody(minitaurMultiBody)
{
m_loggingType = STATE_LOGGING_MINITAUR;
m_motorIdList.resize(motorIdList.size());
for (int m=0;m<motorIdList.size();m++)
{
m_motorIdList[m] = motorIdList[m];
}
btAlignedObjectArray<std::string> structNames;
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
structNames.push_back("t");
structNames.push_back("r");
structNames.push_back("p");
structNames.push_back("y");
structNames.push_back("q0");
structNames.push_back("q1");
structNames.push_back("q2");
structNames.push_back("q3");
structNames.push_back("q4");
structNames.push_back("q5");
structNames.push_back("q6");
structNames.push_back("q7");
structNames.push_back("u0");
structNames.push_back("u1");
structNames.push_back("u2");
structNames.push_back("u3");
structNames.push_back("u4");
structNames.push_back("u5");
structNames.push_back("u6");
structNames.push_back("u7");
structNames.push_back("dx");
structNames.push_back("mo");
m_structTypes = "IffffffffffffffffffffB";
const char* fileNameC = fileName.c_str();
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
}
virtual void stop()
{
if (m_logFileHandle)
{
closeMinitaurLogFile(m_logFileHandle);
m_logFileHandle = 0;
}
}
virtual void logState(btScalar timeStep)
{
if (m_logFileHandle)
{
btVector3 pos = m_minitaurMultiBody->getBasePos();
MinitaurLogRecord logData;
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1};
btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
btMatrix3x3 mat(orn);
btScalar roll=0;
btScalar pitch=0;
btScalar yaw = 0;
mat.getEulerZYX(yaw,pitch,roll);
logData.m_values.push_back(m_loggingTimeStamp);
logData.m_values.push_back((float)roll);
logData.m_values.push_back((float)pitch);
logData.m_values.push_back((float)yaw);
for (int i=0;i<8;i++)
{
float jointAngle = (float)motorDir[i]*m_minitaurMultiBody->getJointPos(m_motorIdList[i]);
logData.m_values.push_back(jointAngle);
}
for (int i=0;i<8;i++)
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_minitaurMultiBody->getLink(m_motorIdList[i]).m_userPtr;
if (motor && timeStep>btScalar(0))
{
btScalar force = motor->getAppliedImpulse(0)/timeStep;
logData.m_values.push_back((float)force);
}
}
//x is forward component, estimated speed forward
float xd_speed = m_minitaurMultiBody->getBaseVel()[0];
logData.m_values.push_back(xd_speed);
char mode = 6;
logData.m_values.push_back(mode);
//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
appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
fflush(m_logFileHandle);
m_loggingTimeStamp++;
}
}
};
struct PhysicsServerCommandProcessorInternalData
@@ -577,7 +713,8 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
btAlignedObjectArray<InternalStateLogger*> m_stateLoggers;
int m_stateLoggersUniqueId;
struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey;
@@ -622,6 +759,7 @@ struct PhysicsServerCommandProcessorInternalData
m_collisionConfiguration(0),
m_dynamicsWorld(0),
m_remoteDebugDrawer(0),
m_stateLoggersUniqueId(0),
m_guiHelper(0),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false),
@@ -758,6 +896,22 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
delete m_data;
}
void logCallback(btDynamicsWorld *world, btScalar timeStep)
{
PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
proc->logObjectStates(timeStep);
}
void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
{
for (int i=0;i<m_data->m_stateLoggers.size();i++)
{
m_data->m_stateLoggers[i]->logState(timeStep);
}
}
void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
{
@@ -804,6 +958,19 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this);
}
void PhysicsServerCommandProcessor::deleteStateLoggers()
{
for (int i=0;i<m_data->m_stateLoggers.size();i++)
{
m_data->m_stateLoggers[i]->stop();
delete m_data->m_stateLoggers[i];
}
m_data->m_stateLoggers.clear();
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -838,6 +1005,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
{
deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies();
deleteStateLoggers();
m_data->m_userConstraints.clear();
m_data->m_saveWorldBodyData.clear();
@@ -1451,7 +1619,85 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
#endif
case CMD_STATE_LOGGING:
{
serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
hasStatus = true;
if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
{
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR)
{
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
//either provide the minitaur by object unique Id, or search for first multibody with 8 motors...
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
{
int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0];
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body)
{
if (body->m_multiBody)
{
btAlignedObjectArray<std::string> motorNames;
motorNames.push_back("motor_front_leftR_joint");
motorNames.push_back("motor_front_leftL_joint");
motorNames.push_back("motor_back_leftR_joint");
motorNames.push_back("motor_back_leftL_joint");
motorNames.push_back("motor_front_rightL_joint");
motorNames.push_back("motor_front_rightR_joint");
motorNames.push_back("motor_back_rightL_joint");
motorNames.push_back("motor_back_rightR_joint");
btAlignedObjectArray<int> motorIdList;
for (int m=0;m<motorNames.size();m++)
{
for (int i=0;i<body->m_multiBody->getNumLinks();i++)
{
std::string jointName;
if (body->m_multiBody->getLink(i).m_jointName)
{
jointName = body->m_multiBody->getLink(i).m_jointName;
}
if (motorNames[m]==jointName)
{
motorIdList.push_back(i);
}
}
}
if (motorIdList.size()==8)
{
int loggerUid = m_data->m_stateLoggersUniqueId++;
MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList);
m_data->m_stateLoggers.push_back(logger);
serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
}
}
}
}
}
}
if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0)
{
serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
for (int i=0;i<m_data->m_stateLoggers.size();i++)
{
if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId)
{
m_data->m_stateLoggers[i]->stop();
delete m_data->m_stateLoggers[i];
m_data->m_stateLoggers.removeAtIndex(i);
}
}
}
break;
}
case CMD_SET_VR_CAMERA_STATE:
{
@@ -4693,7 +4939,6 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
void PhysicsServerCommandProcessor::resetSimulation()
{
//clean up all data
deleteCachedInverseDynamicsBodies();
if (m_data && m_data->m_guiHelper)
{

10
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -20,6 +20,8 @@ class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
struct PhysicsServerCommandProcessorInternalData* m_data;
//todo: move this to physics client side / Python
void createDefaultRobotAssets();
@@ -44,6 +46,7 @@ protected:
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
void deleteCachedInverseDynamicsBodies();
void deleteCachedInverseKinematicsBodies();
void deleteStateLoggers();
public:
PhysicsServerCommandProcessor();
@@ -84,10 +87,15 @@ public:
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual void removePickingConstraint();
//logging /playback the shared memory commands
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
//logging of object states (position etc)
void logObjectStates(btScalar timeStep);
void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents);
void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId);

25
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -613,6 +613,13 @@ enum eVRCameraEnums
VR_CAMERA_ROOT_TRACKING_OBJECT=4
};
enum eStateLoggingEnums
{
STATE_LOGGING_START_LOG=1,
STATE_LOGGING_STOP_LOG=2,
STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
};
struct VRCameraState
{
double m_rootPosition[3];
@@ -621,6 +628,21 @@ struct VRCameraState
};
struct StateLoggingRequest
{
char m_fileName[MAX_FILENAME_LENGTH];
int m_logType;//Minitaur, generic robot, VR states
int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_loggingUniqueId;
};
struct StateLoggingResultArgs
{
int m_loggingUniqueId;
};
struct SharedMemoryCommand
{
int m_type;
@@ -662,6 +684,7 @@ struct SharedMemoryCommand
struct RequestRaycastIntersections m_requestRaycastIntersections;
struct LoadBunnyArgs m_loadBunnyArguments;
struct VRCameraState m_vrCameraStateArguments;
struct StateLoggingRequest m_stateLoggingArguments;
};
};
@@ -722,6 +745,8 @@ struct SharedMemoryStatus
struct b3UserConstraint m_userConstraintResultArgs;
struct SendVREvents m_sendVREvents;
struct SendRaycastHits m_raycastHits;
struct StateLoggingResultArgs m_stateLoggingResultArgs;
};
};

11
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -48,6 +48,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_VR_EVENTS_DATA,
CMD_SET_VR_CAMERA_STATE,
CMD_SYNC_BODY_INFO,
CMD_STATE_LOGGING,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -122,6 +123,9 @@ enum EnumSharedMemoryServerStatus
CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
CMD_SYNC_BODY_INFO_COMPLETED,
CMD_SYNC_BODY_INFO_FAILED,
CMD_STATE_LOGGING_COMPLETED,
CMD_STATE_LOGGING_START_COMPLETED,
CMD_STATE_LOGGING_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -301,6 +305,13 @@ enum
CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
};
enum
{
STATE_LOGGING_MINITAUR = 0,
STATE_LOGGING_GENERIC_ROBOT = 1,
STATE_LOGGING_VR_CONTROLLERS = 2,
STATE_LOGGING_COMMANDS = 3,
};
struct b3ContactInformation

4
examples/SharedMemory/premake4.lua
View File

@@ -76,7 +76,9 @@ myfiles =
"../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
"../Importers/ImportMJCFDemo/BulletMJCFImporter.h",
"../Utils/b3ResourcePath.cpp",
"../Utils/b3Clock.cpp",
"../Utils/b3Clock.cpp",
"../Utils/RobotLoggingUtil.cpp",
"../Utils/RobotLoggingUtil.h",
"../Utils/ChromeTraceUtil.cpp",
"../Utils/ChromeTraceUtil.h",
"../../Extras/Serialize/BulletWorldImporter/*",

4
examples/SharedMemory/udp/premake4.lua
View File

@@ -103,7 +103,9 @@ myfiles =
"../../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../Importers/ImportURDFDemo/URDF2Bullet.h",
"../../Utils/b3ResourcePath.cpp",
"../../Utils/b3Clock.cpp",
"../../Utils/b3Clock.cpp",
"../../Utils/RobotLoggingUtil.cpp",
"../../Utils/RobotLoggingUtil.h",
"../../../Extras/Serialize/BulletWorldImporter/*",
"../../../Extras/Serialize/BulletFileLoader/*",
"../../Importers/ImportURDFDemo/URDFImporterInterface.h",

3
examples/pybullet/CMakeLists.txt
View File

@@ -54,6 +54,9 @@ SET(pybullet_SRCS
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp

15
examples/pybullet/logMinitaur.py Normal file
View File

@@ -0,0 +1,15 @@
import pybullet as p
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0) :
p.connect(p.GUI)
p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"LOG00048.TXT",[quadruped])
p.stepSimulation()
p.stepSimulation()
p.stepSimulation()
p.stepSimulation()
p.stepSimulation()
p.stopStateLogging(logId)

2
examples/pybullet/premake4.lua
View File

@@ -101,6 +101,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/SharedMemory/PosixSharedMemory.h",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",

109
examples/pybullet/pybullet.c
View File

@@ -2590,6 +2590,102 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
return Py_None;
}
static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject *keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3PhysicsClientHandle sm = 0;
int loggingType = -1;
char* fileName = 0;
PyObject* objectUniqueIdsObj = 0;
static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL };
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(sm);
b3StateLoggingStart(commandHandle,loggingType,fileName);
if (objectUniqueIdsObj)
{
PyObject* seq = PySequence_Fast(objectUniqueIdsObj, "expected a sequence of object unique ids");
if (seq)
{
int len = PySequence_Size(objectUniqueIdsObj);
int i;
for ( i=0;i<len;i++)
{
int objectUid = pybullet_internalGetFloatFromSequence(seq, i);
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
}
Py_DECREF(seq);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
{
int loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
PyObject* loggingUidObj = PyInt_FromLong(loggingUniqueId);
return loggingUidObj;
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int loggingId=-1;
b3PhysicsClientHandle sm = 0;
static char *kwlist[] = { "loggingId", "physicsClientId", NULL };
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,
&loggingId,&physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (loggingId>=0)
{
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(sm);
b3StateLoggingStop(commandHandle, loggingId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds)
{
@@ -4967,8 +5063,14 @@ static PyMethodDef SpamMethods[] = {
"Set properties of the VR Camera such as its root transform "
"for teleporting or to track objects (camera inside a vehicle for example)."
},
{ "startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS,
"Start logging of state, such as robot base position, orientation, joint positions etc. "
"Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), "
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"
},
{ "stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
"Stop logging of robot state, given a loggingUniqueId."
},
{ "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
"Cast a ray and return the first object hit, if any. "
"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"
@@ -5054,6 +5156,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR);
PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT);
PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);

2
test/CMakeLists.txt
View File

@@ -3,5 +3,5 @@ IF(BUILD_BULLET3)
SUBDIRS( InverseDynamics SharedMemory )
ENDIF(BUILD_BULLET3)
SUBDIRS( gtest-1.7.0 collision BulletDynamics/pendulum )
SUBDIRS( gtest-1.7.0 collision RobotLogging BulletDynamics/pendulum )

81
test/RobotLogging/main.cpp Normal file
View File

@@ -0,0 +1,81 @@
#include "Utils/RobotLoggingUtil.h"
#ifndef ENABLE_GTEST
#include <assert.h>
#define ASSERT_EQ(a,b) assert((a)==(b));
#else
#include <gtest/gtest.h>
#define printf
#endif
void testMinitaurLogging()
{
const char* fileName = "d:/logTest.txt";
btAlignedObjectArray<std::string> structNames;
std::string structTypes;
btAlignedObjectArray<MinitaurLogRecord> logRecords;
bool verbose = false;
int status = readMinitaurLogFile(fileName, structNames, structTypes, logRecords, verbose);
for (int i=0;i<logRecords.size();i++)
{
for (int j=0;j<structTypes.size();j++)
{
switch (structTypes[j])
{
case 'I':
{
int v = logRecords[i].m_values[j].m_intVal;
printf("record %d, %s = %d\n",i,structNames[j].c_str(),v);
break;
}
case 'f':
{
float v = logRecords[i].m_values[j].m_floatVal;
printf("record %d, %s = %f\n",i,structNames[j].c_str(),v);
break;
}
case 'B':
{
int v = logRecords[i].m_values[j].m_charVal;
printf("record %d, %s = %d\n",i,structNames[j].c_str(),v);
break;
}
default:
{
}
}
}
}
}
#ifdef ENABLE_GTEST
TEST(RobotLoggingTest, LogMinitaur) {
testMinitaurLogging();
}
#endif
int main(int argc, char* argv[])
{
//b3SetCustomPrintfFunc(myprintf);
//b3SetCustomWarningMessageFunc(myprintf);
#ifdef ENABLE_GTEST
#if _MSC_VER
_CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
//void *testWhetherMemoryLeakDetectionWorks = malloc(1);
#endif
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
#else
testMinitaurLogging();
#endif
}

2
test/SharedMemory/CMakeLists.txt
View File

@@ -56,6 +56,8 @@ ENDIF()
../../examples/SharedMemory/PosixSharedMemory.h
../../examples/Utils/b3ResourcePath.cpp
../../examples/Utils/b3ResourcePath.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
../../examples/OpenGLWindow/SimpleCamera.cpp

6
test/SharedMemory/premake4.lua
View File

@@ -84,6 +84,8 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
@@ -158,6 +160,8 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/OpenGLWindow/SimpleCamera.h",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
@@ -259,6 +263,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",