diff --git a/examples/CommonInterfaces/CommonParameterInterface.h b/examples/CommonInterfaces/CommonParameterInterface.h index 85d138d59..6243cc8c5 100644 --- a/examples/CommonInterfaces/CommonParameterInterface.h +++ b/examples/CommonInterfaces/CommonParameterInterface.h @@ -32,12 +32,32 @@ struct SliderParams }; +typedef void (*ButtonParamChangedCallback) (int buttonId, bool buttonState, void* userPointer); + +struct ButtonParams +{ + const char* m_name; + int m_buttonId; + void* m_userPointer; + + ButtonParamChangedCallback m_callback; + ButtonParams(const char* name, int buttonId, bool isTrigger) + :m_name(name), + m_buttonId(buttonId), + m_userPointer(0), + m_callback(0) + { + } +}; + struct CommonParameterInterface { virtual ~CommonParameterInterface() {} virtual void registerSliderFloatParameter(SliderParams& params)=0; + virtual void registerButtonParameter(ButtonParams& params)=0; + virtual void syncParameters()=0; virtual void removeAllParameters()=0; virtual void setSliderValue(int sliderIndex, double sliderValue)=0; diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp index 8c38528b3..322fca33c 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp @@ -1,6 +1,27 @@ #include "GwenParameterInterface.h" #include "gwenInternalData.h" +struct MyButtonEventHandler : public Gwen::Event::Handler +{ + ButtonParamChangedCallback m_callback; + void* m_userPointer; + int m_buttonId; + + MyButtonEventHandler(ButtonParamChangedCallback callback, int buttonId, void* userPointer) + :m_callback(callback), + m_userPointer(userPointer), + m_buttonId(buttonId) + { + } + + void onButtonPress( Gwen::Controls::Base* pControl ) + { + if (m_callback) + { + (*m_callback)(m_buttonId, true, m_userPointer); + } + } +}; template @@ -62,6 +83,9 @@ struct GwenParameters { b3AlignedObjectArray*> m_sliderEventHandlers; b3AlignedObjectArray m_sliders; + + b3AlignedObjectArray m_buttons; + b3AlignedObjectArray m_buttonEventHandlers; b3AlignedObjectArray m_textLabels; int m_savedYposition; }; @@ -102,6 +126,24 @@ void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue) } #include + +void GwenParameterInterface::registerButtonParameter(ButtonParams& params) +{ + Gwen::Controls::TextBox* label = new Gwen::Controls::TextBox(m_gwenInternalData->m_demoPage->GetPage()); + + Gwen::Controls::Button* button = new Gwen::Controls::Button(m_gwenInternalData->m_demoPage->GetPage()); + MyButtonEventHandler* handler = new MyButtonEventHandler(params.m_callback,params.m_buttonId,params.m_userPointer); + button->SetText(params.m_name); + button->onPress.Add( handler, &MyButtonEventHandler::onButtonPress ); + + m_paramInternalData->m_buttons.push_back(button); + m_paramInternalData->m_buttonEventHandlers.push_back(handler); + + button->SetPos( 10, m_gwenInternalData->m_curYposition ); + m_gwenInternalData->m_curYposition+=22; + +} + void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) { Gwen::Controls::TextBox* label = new Gwen::Controls::TextBox(m_gwenInternalData->m_demoPage->GetPage()); diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h index d168c7e5a..6ee4a54cd 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h +++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h @@ -12,6 +12,8 @@ struct GwenParameterInterface : public CommonParameterInterface GwenParameterInterface(struct GwenInternalData* gwenInternalData); virtual ~GwenParameterInterface(); virtual void registerSliderFloatParameter(SliderParams& params); + virtual void registerButtonParameter(ButtonParams& params); + virtual void setSliderValue(int sliderIndex, double sliderValue); virtual void syncParameters(); virtual void removeAllParameters(); diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 0f9ecce61..37d01c739 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -5,6 +5,8 @@ #include "PosixSharedMemory.h" #include "Win32SharedMemory.h" #include "SharedMemoryCommon.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + class PhysicsClient : public SharedMemoryCommon { @@ -12,6 +14,13 @@ class PhysicsClient : public SharedMemoryCommon SharedMemoryExampleData* m_testBlock1; int m_counter; bool m_wantsTermination; + btAlignedObjectArray m_userCommandRequests; + + bool m_serverLoadUrdfOK; + bool m_waitingForServer; + + + public: @@ -35,13 +44,56 @@ public: { return m_wantsTermination; } + void submitCommand(int command); + }; + +void MyCallback(int buttonId, bool buttonState, void* userPtr) +{ + PhysicsClient* cl = (PhysicsClient*) userPtr; + switch (buttonId) + { + case CMD_LOAD_URDF: + { + cl->submitCommand(CMD_LOAD_URDF); + break; + } + + case CMD_STEP_FORWARD_SIMULATION: + { + cl->submitCommand(CMD_STEP_FORWARD_SIMULATION); + break; + } + case CMD_SHUTDOWN: + { + cl->submitCommand(CMD_SHUTDOWN); + break; + } + + default: + { + b3Error("Unknown buttonId"); + btAssert(0); + } + }; +} + + +void PhysicsClient::submitCommand(int command) +{ + m_userCommandRequests.push_back(command); + b3Printf("User submitted command request %d (outstanding %d)\n",command, m_userCommandRequests.size()); +} + + PhysicsClient::PhysicsClient(GUIHelperInterface* helper) :SharedMemoryCommon(helper), m_testBlock1(0), m_counter(0), -m_wantsTermination(false) +m_wantsTermination(false), +m_serverLoadUrdfOK(false), +m_waitingForServer(false) { b3Printf("Started PhysicsClient"); #ifdef _WIN32 @@ -58,8 +110,34 @@ PhysicsClient::~PhysicsClient() delete m_sharedMemory; } + void PhysicsClient::initPhysics() { + { + bool isTrigger = false; + ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + + { + bool isTrigger = false; + ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + + { + bool isTrigger = false; + ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + + m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); if (m_testBlock1) { @@ -69,16 +147,10 @@ void PhysicsClient::initPhysics() b3Error("Error: please start server before client"); m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_testBlock1 = 0; - } else - { - //submit a 'load urdf' command to get things started - - b3Printf("Client created CMD_LOAD_URDF"); - m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF; - sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf"); - m_testBlock1->m_numClientCommands++; - } + { + b3Printf("Shared Memory status is OK"); + } } } @@ -102,30 +174,20 @@ void PhysicsClient::stepSimulation(float deltaTime) switch (serverCmd.m_type) { + case CMD_URDF_LOADING_COMPLETED: + { + m_serverLoadUrdfOK = true; + b3Printf("Server loading the URDF OK"); + break; + } case CMD_STEP_FORWARD_SIMULATION_COMPLETED: - case CMD_URDF_LOADING_COMPLETED: { - //submit a 'step simulation' request - - if (m_counter<10) - { - m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; - m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; - m_testBlock1->m_numClientCommands++; - m_counter++; - - } else - { - m_wantsTermination = true; - m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; - m_testBlock1->m_numClientCommands++; - } - break; } case CMD_URDF_LOADING_FAILED: { b3Printf("Server failed loading the URDF..."); + m_serverLoadUrdfOK = false; break; } default: @@ -134,12 +196,83 @@ void PhysicsClient::stepSimulation(float deltaTime) btAssert(0); } }; + + m_testBlock1->m_numProcessedServerCommands++; + + if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands) + { + m_waitingForServer = false; + } else + { + m_waitingForServer = true; + } } - - } - + if (!m_waitingForServer) + { + //process outstanding requests + if (m_userCommandRequests.size()) + { + b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); + int command = m_userCommandRequests[0]; + m_userCommandRequests.remove(command); + switch (command) + { + case CMD_LOAD_URDF: + { + if (!m_serverLoadUrdfOK) + { + m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF; + sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf"); + m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false; + m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true; + + m_testBlock1->m_numClientCommands++; + b3Printf("Client created CMD_LOAD_URDF"); + m_waitingForServer = true; + } else + { + b3Warning("Server already loaded URDF, no client command submitted"); + } + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + if (m_serverLoadUrdfOK) + { + + m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; + m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; + m_testBlock1->m_numClientCommands++; + b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++); + m_waitingForServer = true; + } else + { + b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted"); + } + break; + } + case CMD_SHUTDOWN: + { + m_wantsTermination = true; + m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; + m_testBlock1->m_numClientCommands++; + m_waitingForServer = true; + m_serverLoadUrdfOK = false; + b3Printf("client created CMD_SHUTDOWN\n"); + break; + } + default: + { + b3Error("unknown command requested"); + } + } + } + } + + } + } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 9f56f5036..1f0165d4f 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -15,6 +15,8 @@ class PhysicsServer : public SharedMemoryCommon { SharedMemoryInterface* m_sharedMemory; SharedMemoryExampleData* m_testBlock1; + + btAlignedObjectArray m_jointFeedbacks; bool m_wantsShutdown; public: @@ -34,10 +36,10 @@ public: virtual void resetCamera() { - float dist = 1; + float dist = 5; float pitch = 50; float yaw = 35; - float targetPos[3]={-3,2.8,-2.5}; + float targetPos[3]={0,0,0};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } @@ -83,7 +85,16 @@ PhysicsServer::~PhysicsServer() void PhysicsServer::initPhysics() { + ///for this testing we use Z-axis up + int upAxis = 2; + m_guiHelper->setUpAxis(upAxis); + createEmptyDynamicsWorld(); + //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + btVector3 grav(0,0,0); + grav[upAxis] = -9.8; + this->m_dynamicsWorld->setGravity(grav); m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); @@ -135,8 +146,29 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); btMultiBody* mb = creation.getBulletMultiBody(); + if (useMultiBody) + { + if (mb) + { + return true; + } else + { + b3Warning("No multibody loaded from URDF"); + return false; + } + } else + { + for (int i=0;igetNumConstraints();i++) + { + btTypedConstraint* c = m_dynamicsWorld->getConstraint(i); + btJointFeedback* fb = new btJointFeedback(); + m_jointFeedbacks.push_back(fb); + c->setJointFeedback(fb); - return true; + } + return true; + } + } return false; @@ -169,8 +201,9 @@ void PhysicsServer::stepSimulation(float deltaTime) //load the actual URDF and send a report: completed or failed + bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName, - btVector3(0,0,0), btQuaternion(0,0,0,1),true,true ); + btVector3(0,0,0), btQuaternion(0,0,0,1),clientCmd.m_urdfArguments.m_useMultiBody,clientCmd.m_urdfArguments.m_useFixedBase); SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; if (completedOk) @@ -195,7 +228,24 @@ void PhysicsServer::stepSimulation(float deltaTime) serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_testBlock1->m_numServerCommands++; - + + //now we send back the actual q, q' and force/torque and IMU sensor values + for (int i=0;im_appliedForceBodyA.x(), + m_jointFeedbacks[i]->m_appliedForceBodyA.y(), + m_jointFeedbacks[i]->m_appliedForceBodyA.z(), + m_jointFeedbacks[i]->m_appliedTorqueBodyA.x(), + m_jointFeedbacks[i]->m_appliedTorqueBodyA.y(), + m_jointFeedbacks[i]->m_appliedTorqueBodyA.z(), + m_jointFeedbacks[i]->m_appliedForceBodyB.x(), + m_jointFeedbacks[i]->m_appliedForceBodyB.y(), + m_jointFeedbacks[i]->m_appliedForceBodyB.z(), + m_jointFeedbacks[i]->m_appliedTorqueBodyB.x(), + m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(), + m_jointFeedbacks[i]->m_appliedTorqueBodyB.z()); + } break; } case CMD_SHUTDOWN: diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index cd060788d..c69dad4ec 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -31,6 +31,8 @@ enum SharedMemoryClientCommand{ struct UrdfCommandArgument { char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; + bool m_useMultiBody; + bool m_useFixedBase; }; struct StepSimulationCommandArgument diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index dd96ac4d8..bafb841a3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -13,8 +13,8 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod m_jacSizeA(0), m_jacSizeBoth(0), m_isUnilateral(isUnilateral), - m_maxAppliedImpulse(100), - m_numDofsFinalized(-1) + m_numDofsFinalized(-1), + m_maxAppliedImpulse(100) { } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 19d0543ef..759509a1d 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -161,7 +161,7 @@ public: } virtual btScalar getMargin() const { - return getMargin(); + return btConvexInternalShape::getMargin(); } };