From 59b32b7af16d06662dd45646831b1c92a79d8d55 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 8 Apr 2016 18:17:17 -0700 Subject: [PATCH] re-enable command log and playback in physics server report applied motor torque in physics server --- data/multibody.bullet | Bin 14520 -> 14584 bytes .../PhysicsServerCommandProcessor.cpp | 60 ++++++++++++------ .../PhysicsServerCommandProcessor.h | 1 + .../PhysicsServerSharedMemory.cpp | 19 +----- examples/SharedMemory/SharedMemoryCommands.h | 3 +- examples/SharedMemory/main.cpp | 3 + .../Featherstone/btMultiBodyConstraint.h | 1 + 7 files changed, 49 insertions(+), 38 deletions(-) diff --git a/data/multibody.bullet b/data/multibody.bullet index abbacacce01a1d2da01995c7f1d3372be5c2c9f5..04455f0d581801063808b200d0285610993fd875 100644 GIT binary patch delta 2239 zcmb7_X>3$g7>2()OV63oIi@W_tffY=OpGOlwF?wzEg-g{;-9KuK~1EFLNQfDmMQML z3`YemDk^T^1_OvlSyez=#eYVrEG|$1H^87V;&a+*JCe}oq)*Or-|zk2bMM^g`k5zZ zF3k4wVtp3t^F-tnpV;nfTN}u-U#ZmCU)etK+bnNFY+o?F5P|!2j`nI_X<&J6<0iW* zSQV=e+B{!gnzQKmAQ*0!`7-QepSJS@+HUY=#b&47nB8t90Y;seyW!NSKXbgqI*oCw zzkRXg3}aW6L{W0Iu_0DDT<{9*B7c!x6Rb?1P+E5FNjkWuj{fofflSZq6E6v#<@u!0 zPDqLP3dPPz$+cGnQtgTq^4MJ|k&cy1EH+zR$aab9o$gNG+|*R;wws$OMu@Feb0#@e z4{-Ovo#*ew7-a5C+W#YIKa|*a*E{qn%$=9>ar+L>#!|1b8$;SI)wPr9-N>f<)bqEw zZ6-33aU^*flk+96G1RwRPiTs})5NER>b#KaUpBsEz!BS;dagGw9_(>Qh3lr=E;Us* zPpPS`o_gjc5?GnUo%Q3fA-&9vUW4dj* zpgbPwyUlNlBI)+`emS;0l4e``W!dv1=h$hHP<(Rax`0|Dm9k!@+N~Fl^MC29kB9U2 z`=h;hdUIhO%z*hY6QWQL3*c_J2kwRY;C^@j9)yRutq;Q^@F+Y6k8?kAWw7)Tg}1_6 z;jQpicq_aW-U@FO#av;oFjtr>%oXMebJdOc5T4*5c|wdVl%dQAGY>P*m$PM{#3Wr7 zv9|CeJOxj~LRbt-;2C&UqDmTQv=p9$=ivod1~0-(@G>V_E(M%s0LB>@8;lLc24jP< z3t=!e7#oZY#u{IZuf|v7tMS$NYJ4@my4^S0cB=|GK|o%CSEYzt`SKdP&b7Ymy#Xs= zCA>+Xx8QB&@4&mvS23=ZVbl$$ZutMIi>{Vp@)l57aE3xnL!H!6CmM>xWrX-?yoT0m zVI6OaAHOv?u7!0nDsjDIU4qK#d&+yTUII~hpY$8x15UCL8etP`hAprawn=HCFq6Ve zSNI?6E+vKw)Y&kpPSY={X{c$aX{c$aX{c!^vR%p~^ToN*`Yf=@W%r_dy05hkC&pfl(UI+LWkOoEs; zQ}Y=OKZo7$g^Xi=Y+_%tui4k^UoICBAiXPbB7n7na-(7Snh<+P+5+FeUf2iwVS8uD zupD6iEqn*x!w+x}4nZsY2#4Vabw}YR_!)kIUkUr0loRB%0|wj)?XbbQQ9}^ZO^}Ye zUYOxaQdsx&FhB2=W%(sC3Fw8%}0#-N~PBZ@?ztri85F5Vyt z)Te@qQWU`h72K+T+-DIGNc_iu%1yaJ(O_btpSK4rMAXTC_8s&6o%?3?V%74h*_l?m z+8gJ1|Kdu~Ax~F*!cMLIGt+g>rCVCVyyHcrZ*5f|d^?j-P!1Dd3_Jjjz#UKlk3uzM zL0ia!#I>+;^%^tJftw)ij;woU0FM;U3*&Eio^zMt6h+=MNA1U?(EZyL}OE2=Sv{JO?4d`lLGne@E z3||f#%X~pApnvcMJ?2#CdS9E^B-d_Z<>_*}c^Yn}@>$C$J9iA~*HU@Mi;nsxf&7qJ|)xA zb;{!{9nvdrmHs2Ol{GoitVyFC8IXR`ZI23#s~j_abfnV_``zy9(1b{8pu~za&D`hH z{_Fvf;o0lm`p#fWT^LN$?Sg(iKG;IHVBRlyot_s=iOde(9bSSN z@G`stGhr6Y7LU|&@-ujHYNRQC9sD|B96LC6aBPxraO~jN!Lfs5g=2+dg=2+dg=2+d zg=2+d6?be7TvrP4$g7}v{QMO08oW-Av)OwC=D=KdlZ4)awjFzNw)~QqHEYG0xAL~>{KjXnC$ALr??5cfY9&3`y3j#6ti|%JFH9M_Y>=0 zsEj^c7QzQOT?C8aLkjr_K88MfQo>M zfQo>MfQo=3OQn2BTB(|^ESL`eH z2k?S2KSd(p%L%L_f%UKfHo_*@3|n9;G{Cl8zP_cB?XUxO!gsI>mU1svgD>T41xON_iCONX(gK&@D z^)HDGy6PvfX|+wOZHlpzIz9NRpG2Wk=oC7Yq#Hu#yU}649DsxPI7HDjVkmXTo{|G3 zbr7RNyv#Bf4?{gvz!9i|qi_rw;W(UtlW+=7!x=aW=kR(Set}=%0{mtI@^Z`|_+hAr z!4-TRfhsr($Dk38!wEPEr{FZ4fwOQ;=X!5JXKR|izDwAukId@Q+_D<=;=*jJMsF`1 z$UjT-qB8eLNzG2xc@R(R4x+ MK;uVabKmIx1_ddl?*IS* 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)