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

fix quadruped, allow user to pick the maximum number of dofs to log

Browse Source
This commit is contained in:
erwincoumans
2017-03-04 15:30:57 -08:00
parent 44b140dd38
commit c7579d7b82
7 changed files with 39 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2585,6 +2585,20 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa
}
return 0;
}
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof)
{
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_MAX_LOG_DOF;
command->m_stateLoggingArguments.m_maxLogDof = maxLogDof;
}
return 0;
}
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -356,9 +356,13 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1856,6 +1856,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
int loggerUid = m_data->m_stateLoggersUniqueId++;
int maxLogDof = 12;
if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF))
{
maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
}
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld);
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
@@ -1863,7 +1868,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
logger->m_filterObjectUniqueId = true;
for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i)
{
logger->m_bodyIdList.push_back(clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]);
int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i];
logger->m_bodyIdList.push_back(objectUniqueId);
}
}

1
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -322,6 +322,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
numCmdSinceSleep1ms = 0;
sleepClock.reset();
}
}
if (sleepClock.getTimeMilliseconds()>1)
{
sleepClock.reset();

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -626,6 +626,7 @@ enum eStateLoggingEnums
STATE_LOGGING_START_LOG=1,
STATE_LOGGING_STOP_LOG=2,
STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
STATE_LOGGING_MAX_LOG_DOF=8
};
struct VRCameraState
@@ -644,6 +645,7 @@ struct StateLoggingRequest
int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_loggingUniqueId;
int m_maxLogDof;
};
struct StateLoggingResultArgs

6
examples/pybullet/kuka_with_cube_playback.py
View File

@@ -16,8 +16,8 @@ def readLogFile(filename, verbose = True):
print('Opened'),
print(filename)
keys = f.readline().rstrip('\n').split(',')
fmt = f.readline().rstrip('\n')
keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record
sz = struct.calcsize(fmt)
@@ -37,7 +37,7 @@ def readLogFile(filename, verbose = True):
# Read data
wholeFile = f.read()
# split by alignment word
chunks = wholeFile.split('\xaa\xbb')
chunks = wholeFile.split(b'\xaa\xbb')
log = list()
for chunk in chunks:
if len(chunk) == sz:

11
examples/pybullet/pybullet.c
View File

@@ -2625,12 +2625,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
int loggingType = -1;
char* fileName = 0;
PyObject* objectUniqueIdsObj = 0;
int maxLogDof=-1;
static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL };
static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL };
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj,&maxLogDof, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -2661,6 +2662,10 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
}
}
if (maxLogDof>0)
{
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);