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

re-enable command log and playback in physics server

Browse Source 
report applied motor torque in physics server
This commit is contained in:
Erwin Coumans
2016-04-08 18:17:17 -07:00
parent d2793ec5c0
commit 59b32b7af1
7 changed files with 49 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
data/multibody.bullet
View File

Binary file not shown.

60
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -642,6 +642,8 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
//motor->setMaxAppliedImpulse(0); //motor->setMaxAppliedImpulse(0);
m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
motor->finalizeMultiDof();
} }
} }
@@ -759,7 +761,21 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
return false; return false;
} }
void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes)
{
if (m_data->m_logPlayback)
{
SharedMemoryCommand clientCmd;
SharedMemoryStatus serverStatus;
bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd);
if (hasCommand)
{
processCommand(clientCmd,serverStatus,bufferServerToClient,bufferSizeInBytes);
}
}
}
@@ -769,22 +785,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool hasStatus = false; bool hasStatus = false;
{ {
#if 0
if (m_data->m_logPlayback)
{
if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands)
{
m_data->m_testBlock1->m_numProcessedServerCommands++;
}
//push a command from log file
bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]);
if (hasCommand)
{
m_data->m_testBlock1->m_numClientCommands++;
}
}
#endif
///we ignore overflow of integer for now ///we ignore overflow of integer for now
{ {
@@ -793,10 +793,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
#if 0 #if 1
if (m_data->m_commandLogger) if (m_data->m_commandLogger)
{ {
m_data->m_commandLogger->logCommand(m_data->m_testBlock1); m_data->m_commandLogger->logCommand(clientCmd);
} }
#endif #endif
@@ -1277,8 +1277,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2];
} }
}
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
if (supportsJointMotor(mb,l))
{
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[l];
if (motorPtr && m_data->m_physicsDeltaTime>btScalar(0))
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] =
force;
//if (force>0)
//{
// b3Printf("force = %f\n", force);
//}
}
}
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -47,6 +47,7 @@ public:
void enableCommandLogging(bool enable, const char* fileName); void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName); void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
}; };

19
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -240,23 +240,8 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
if (m_data->m_isConnected && m_data->m_testBlock1) if (m_data->m_isConnected && m_data->m_testBlock1)
{ {
#if 0 m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_commandProcessor->processLogCommand();
if (m_data->m_logPlayback)
{
if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands)
{
m_data->m_testBlock1->m_numProcessedServerCommands++;
}
//push a command from log file
bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]);
if (hasCommand)
{
m_data->m_testBlock1->m_numClientCommands++;
}
}
#endif
///we ignore overflow of integer for now ///we ignore overflow of integer for now
if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands) if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
{ {

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -194,7 +194,8 @@ struct SendActualStateArgs
//measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z] //measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z]
double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM]; double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
}; };
enum EnumSensorTypes enum EnumSensorTypes

3
examples/SharedMemory/main.cpp
View File

@@ -78,6 +78,9 @@ int main(int argc, char* argv[])
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
}else }else
{ {
// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
// options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
} }

1
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -114,6 +114,7 @@ public:
btAssert(dof>=0); btAssert(dof>=0);
btAssert(dof < getNumRows()); btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse; m_data[dof] = appliedImpulse;
} }
btScalar getAppliedImpulse(int dof) btScalar getAppliedImpulse(int dof)