diff --git a/build3/premake4.lua b/build3/premake4.lua index 59f4f12c7..d04b1274d 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -66,6 +66,13 @@ description = "Enable Lua scipting support in Example Browser" } + newoption { + trigger = "targetdir", + value = "path such as ../bin", + description = "Set the output location for the generated project files" + } + + newoption { trigger = "without-gtest", @@ -129,7 +136,9 @@ -- comment-out for now, URDF reader needs exceptions -- flags { "NoRTTI", "NoExceptions"} -- defines { "_HAS_EXCEPTIONS=0" } - targetdir "../bin" +--printf ( _OPTIONS["targetdir"] ) + + targetdir( _OPTIONS["targetdir"] or "../bin" ) location("./" .. act .. postfix) diff --git a/build3/vs2010.bat b/build3/vs2010.bat index b4f4e764b..7bca3cf71 100644 --- a/build3/vs2010.bat +++ b/build3/vs2010.bat @@ -1,7 +1,18 @@ rem premake4 --with-pe vs2010 rem premake4 --bullet2demos vs2010 -premake4 vs2010 +premake4 --targetdir="../bin" vs2010 +rem premake4 --targetdir="../server2bin" vs2010 +rem cd vs2010 +rem rename 0_Bullet3Solution.sln 0_server.sln +rem cd .. +rem rename vs2010 vs2010_server +rem +rem premake4 --targetdir="../client2bin" vs2010 +rem cd vs2010 +rem rename 0_Bullet3Solution.sln 0_client.sln +rem cd .. +rem rename vs2010 vs2010_client + -mkdir vs2010\cache pause \ No newline at end of file diff --git a/data/hinge.urdf b/data/hinge.urdf new file mode 100644 index 000000000..121eac702 --- /dev/null +++ b/data/hinge.urdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index da4f6a257..840203b62 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -19,6 +19,7 @@ SET(App_ExampleBrowser_SRCS ../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsServerExample.cpp ../SharedMemory/PhysicsClientExample.cpp + ../SharedMemory/RobotControlExample.cpp ../SharedMemory/PosixSharedMemory.cpp ../SharedMemory/Win32SharedMemory.cpp ../BasicDemo/BasicExample.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 858fa80f7..e191b479c 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -32,6 +32,7 @@ #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../SharedMemory/PhysicsServerExample.h" +#include "../SharedMemory/RobotControlExample.h" #include "../SharedMemory/PhysicsClientExample.h" #include "../Constraints/TestHingeTorque.h" #include "../RenderingExamples/TimeSeriesExample.h" @@ -186,6 +187,10 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Experiments"), + + ExampleEntry(1,"Robot Control", "Perform some robot control tasks, using physics server and client that communicate over shared memory", + RobotControlExampleCreateFunc), + ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", PhysicsServerCreateFunc), ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc), diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 8d559d6da..d1dc52dc1 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -39,8 +39,6 @@ public: virtual void stepSimulation(float deltaTime); - bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, - bool useMultiBody, bool useFixedBase); virtual void resetCamera() { @@ -59,7 +57,7 @@ PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper) m_wantsShutdown(false) { b3Printf("Started PhysicsServer\n"); - bool useServer = true; + } diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp new file mode 100644 index 000000000..442e0fa07 --- /dev/null +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -0,0 +1,198 @@ + + + +#include "RobotControlExample.h" + + + +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "PhysicsServer.h" +#include "PhysicsClient.h" +#include "SharedMemoryCommon.h" +//const char* blaatnaam = "basename"; + + + + +class RobotControlExample : public SharedMemoryCommon +{ + PhysicsServerSharedMemory m_physicsServer; + PhysicsClientSharedMemory m_physicsClient; + + bool m_wantsShutdown; + + btAlignedObjectArray m_userCommandRequests; + + void createButton(const char* name, int id, bool isTrigger ); + +public: + + RobotControlExample(GUIHelperInterface* helper); + + virtual ~RobotControlExample(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + void enqueueCommand(int command) + { + m_userCommandRequests.push_back(command); + b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size()); + } + + virtual void resetCamera() + { + float dist = 5; + float pitch = 50; + float yaw = 35; + float targetPos[3]={0,0,0};//-3,2.8,-2.5}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + virtual bool wantsTermination(); +}; + + +void MyCallback2(int buttonId, bool buttonState, void* userPtr) +{ + RobotControlExample* cl = (RobotControlExample*) userPtr; + switch (buttonId) + { + case CMD_LOAD_URDF: + case CMD_CREATE_BOX_COLLISION_SHAPE: + case CMD_REQUEST_ACTUAL_STATE: + case CMD_STEP_FORWARD_SIMULATION: + case CMD_SHUTDOWN: + case CMD_SEND_DESIRED_STATE: + case CMD_SEND_BULLET_DATA_STREAM: + { + cl->enqueueCommand(buttonId); + break; + } + + default: + { + b3Error("Unknown buttonId"); + btAssert(0); + } + }; +} +void RobotControlExample::createButton(const char* name, int buttonId, bool isTrigger ) +{ + ButtonParams button(name,buttonId, isTrigger); + button.m_callback = MyCallback2; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); +} + +RobotControlExample::RobotControlExample(GUIHelperInterface* helper) +:SharedMemoryCommon(helper), +m_wantsShutdown(false) +{ + + bool useServer = true; +} + + + +RobotControlExample::~RobotControlExample() +{ + bool deInitializeSharedMemory = true; + m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); +} + +void RobotControlExample::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] = 0;//-9.8; + this->m_dynamicsWorld->setGravity(grav); + + bool allowSharedMemoryInitialization = true; + m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); + + if (m_guiHelper && m_guiHelper->getParameterInterface()) + { + bool isTrigger = false; + + createButton("Load URDF",CMD_LOAD_URDF, isTrigger); + createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); + createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); + createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); + createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); + createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); + + } else + { + m_userCommandRequests.push_back(CMD_LOAD_URDF); + m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); + m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); + m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); + //m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK); + m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE); + //m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY); + m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION); + m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); + m_userCommandRequests.push_back(CMD_SHUTDOWN); + + } + + if (!m_physicsClient.connect()) + { + b3Warning("Cannot eonnect to physics client"); + } + +} + + +bool RobotControlExample::wantsTermination() +{ + return m_wantsShutdown; +} + + + +void RobotControlExample::stepSimulation(float deltaTime) +{ + m_physicsServer.processClientCommands(); + + if (m_physicsClient.isConnected()) + { + m_physicsClient.processServerStatus(); + + if (m_physicsClient.canSubmitCommand()) + { + if (m_userCommandRequests.size()) + { + b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); + int command = m_userCommandRequests[0]; + + //a manual 'pop_front', we don't use 'remove' because it will re-order the commands + for (int i=1;i