diff --git a/data/multibody.bullet b/data/multibody.bullet index abbacacce..04455f0d5 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b9df7b9ac..62cdeea6e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -642,6 +642,8 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) //motor->setMaxAppliedImpulse(0); m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); + motor->finalizeMultiDof(); + } } @@ -759,7 +761,21 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto 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; { -#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 { @@ -793,10 +793,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; -#if 0 +#if 1 if (m_data->m_commandLogger) { - m_data->m_commandLogger->logCommand(m_data->m_testBlock1); + m_data->m_commandLogger->logCommand(clientCmd); } #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+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_numDegreeOfFreedomU = totalDegreeOfFreedomU; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 31448f292..34b731de8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -47,6 +47,7 @@ public: void enableCommandLogging(bool enable, const char* fileName); void replayFromLogFile(const char* fileName); + void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); }; diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index a19e9904a..af8101464 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -240,23 +240,8 @@ void PhysicsServerSharedMemory::processClientCommands() { if (m_data->m_isConnected && m_data->m_testBlock1) { -#if 0 - 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 + m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + ///we ignore overflow of integer for now if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 198afc092..94e0832be 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -194,7 +194,8 @@ struct SendActualStateArgs //measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z] double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM]; - + + double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM]; }; enum EnumSensorTypes diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index 6aad5da08..fa83a488d 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -78,6 +78,9 @@ int main(int argc, char* argv[]) example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); }else { +// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING; + // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG; + example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 3b32b46e9..137b34d87 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -114,6 +114,7 @@ public: btAssert(dof>=0); btAssert(dof < getNumRows()); m_data[dof] = appliedImpulse; + } btScalar getAppliedImpulse(int dof)