diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h index eeacb64e1..220f9a583 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -3,6 +3,13 @@ #ifndef COMMON_EXAMPLE_INTERFACE_H #define COMMON_EXAMPLE_INTERFACE_H +struct CommandProcessorCreationInterface +{ + virtual class CommandProcessorInterface* createCommandProcessor()=0; + virtual void deleteCommandProcessor(CommandProcessorInterface*)=0; +}; + + struct CommonExampleOptions { struct GUIHelperInterface* m_guiHelper; @@ -11,13 +18,14 @@ struct CommonExampleOptions int m_option; const char* m_fileName; class SharedMemoryInterface* m_sharedMem; - + CommandProcessorCreationInterface* m_commandProcessorCreation; CommonExampleOptions(struct GUIHelperInterface* helper, int option=0) :m_guiHelper(helper), m_option(option), m_fileName(0), - m_sharedMem(0) + m_sharedMem(0), + m_commandProcessorCreation(0) { } diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 23c283fcb..1145572c8 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -175,6 +175,7 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsClientC_API.cpp ../SharedMemory/PhysicsServerExample.cpp + ../SharedMemory/PhysicsServerExampleBullet2.cpp ../SharedMemory/PhysicsClientExample.cpp ../SharedMemory/PosixSharedMemory.cpp ../SharedMemory/Win32SharedMemory.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 04b2112c3..d24fdfd4e 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -39,7 +39,7 @@ #include "../FractureDemo/FractureDemo.h" #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" -#include "../SharedMemory/PhysicsServerExample.h" +#include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsClientExample.h" #include "../Constraints/TestHingeTorque.h" #include "../RenderingExamples/TimeSeriesExample.h" @@ -138,7 +138,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Physics Client-Server"), ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", - PhysicsServerCreateFunc), + PhysicsServerCreateFuncBullet2), ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc), // ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.", diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 88a2abd99..776973724 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -28,6 +28,8 @@ void* ExampleBrowserMemoryFunc(); #include "EmptyExample.h" #include "../SharedMemory/PhysicsServerExample.h" +#include "../SharedMemory/PhysicsServerExampleBullet2.h" + #include "../SharedMemory/PhysicsClientExample.h" #ifndef _WIN32 @@ -124,14 +126,14 @@ static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[]= ExampleEntryPhysicsServer(0,"Robotics Control"), ExampleEntryPhysicsServer(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", - PhysicsServerCreateFunc), + PhysicsServerCreateFuncBullet2), ExampleEntryPhysicsServer(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).", - PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK), + PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_USE_RTC_CLOCK), ExampleEntryPhysicsServer(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.", - PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), + PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), ExampleEntryPhysicsServer(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", - PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), + PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), }; diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index e029b8edf..0ae38bc0f 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -81,6 +81,7 @@ project "App_BulletExampleBrowser" "../SharedMemory/PhysicsClientC_API.cpp", "../SharedMemory/PhysicsClientC_API.h", "../SharedMemory/PhysicsServerExample.cpp", + "../SharedMemory/PhysicsServerExampleBullet2.cpp", "../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/PhysicsServer.cpp", "../SharedMemory/PhysicsServerSharedMemory.cpp", diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index a178db654..7d35a445e 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -32,6 +32,7 @@ SET(RobotSimulator_SRCS ../../examples/SharedMemory/PhysicsServer.cpp ../../examples/SharedMemory/PhysicsServer.h ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp ../../examples/SharedMemory/PhysicsServerSharedMemory.h diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index f2cba47c7..faa6fb281 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -10,6 +10,7 @@ myfiles = "../../examples/SharedMemory/PhysicsServer.cpp", "../../examples/SharedMemory/PhysicsServer.h", "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp", "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.h", diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 3f1517a61..44ceefca3 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -7,6 +7,7 @@ SET(SharedMemory_SRCS PhysicsClientSharedMemory.cpp PhysicsClientExample.cpp PhysicsServerExample.cpp + PhysicsServerExampleBullet2.cpp PhysicsServerSharedMemory.cpp PhysicsServerSharedMemory.h PhysicsServer.cpp diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 0a3bc706c..c554d3595 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -5,7 +5,7 @@ #include "../CommonInterfaces/Common2dCanvasInterface.h" #include "SharedMemoryCommon.h" #include "../CommonInterfaces/CommonParameterInterface.h" - +#include "PhysicsServerCommandProcessor.h" #include "PhysicsClientC_API.h" #include "PhysicsClient.h" //#include "SharedMemoryCommands.h" @@ -37,6 +37,7 @@ class PhysicsClientExample : public SharedMemoryCommon { protected: b3PhysicsClientHandle m_physicsClientHandle; + //this m_physicsServer is only used when option eCLIENTEXAMPLE_SERVER is enabled PhysicsServerSharedMemory m_physicsServer; @@ -520,10 +521,26 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } +struct Bullet2CommandProcessorCreation3 : public CommandProcessorCreationInterface +{ + virtual class CommandProcessorInterface* createCommandProcessor() + { + PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor; + return proc; + } + + virtual void deleteCommandProcessor(CommandProcessorInterface* proc) + { + delete proc; + } +}; + +static Bullet2CommandProcessorCreation3 sB2PC2; PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper, int options) :SharedMemoryCommon(helper), m_physicsClientHandle(0), +m_physicsServer(&sB2PC2,0,0), m_wantsTermination(false), m_sharedMemoryKey(SHARED_MEMORY_KEY), m_selectedBody(-1), @@ -565,6 +582,7 @@ PhysicsClientExample::~PhysicsClientExample() m_canvas->destroyCanvas(m_canvasSegMaskIndex); } + b3Printf("~PhysicsClientExample\n"); } diff --git a/examples/SharedMemory/PhysicsCommandProcessorInterface.h b/examples/SharedMemory/PhysicsCommandProcessorInterface.h index 4bfebf9f3..609d04cd3 100644 --- a/examples/SharedMemory/PhysicsCommandProcessorInterface.h +++ b/examples/SharedMemory/PhysicsCommandProcessorInterface.h @@ -31,6 +31,7 @@ public: class btVector3; +class btQuaternion; class CommandProcessorInterface : public PhysicsCommandProcessorInterface { @@ -41,6 +42,7 @@ public: virtual void syncPhysicsToGraphics()=0; virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0; virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0; + virtual bool isRealTimeSimulationEnabled() const=0; virtual void enableCommandLogging(bool enable, const char* fileName)=0; virtual void replayFromLogFile(const char* fileName)=0; @@ -50,6 +52,11 @@ public: virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; virtual void removePickingConstraint()=0; + virtual const btVector3& getVRTeleportPosition() const=0; + virtual void setVRTeleportPosition(const btVector3& vrReleportPos)=0; + + virtual const btQuaternion& getVRTeleportOrientation() const=0; + virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn)=0; }; #endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index bf33bea2e..d8eac2542 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -2,25 +2,44 @@ #include "PhysicsServerSharedMemory.h" #include "PhysicsClientSharedMemory.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" - +#include "PhysicsServerCommandProcessor.h" +#include "../CommonInterfaces/CommonExampleInterface.h" struct PhysicsLoopBackInternalData { + CommandProcessorInterface* m_commandProcessor; PhysicsClientSharedMemory* m_physicsClient; PhysicsServerSharedMemory* m_physicsServer; DummyGUIHelper m_noGfx; PhysicsLoopBackInternalData() - :m_physicsClient(0), + :m_commandProcessor(0), + m_physicsClient(0), m_physicsServer(0) { } }; +struct Bullet2CommandProcessorCreation2 : public CommandProcessorCreationInterface +{ + virtual class CommandProcessorInterface* createCommandProcessor() + { + PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor; + return proc; + } + + virtual void deleteCommandProcessor(CommandProcessorInterface* proc) + { + delete proc; + } +}; + +static Bullet2CommandProcessorCreation2 sB2Proc; + PhysicsLoopBack::PhysicsLoopBack() { m_data = new PhysicsLoopBackInternalData; - m_data->m_physicsServer = new PhysicsServerSharedMemory(); + m_data->m_physicsServer = new PhysicsServerSharedMemory(&sB2Proc, 0,0); m_data->m_physicsClient = new PhysicsClientSharedMemory(); } @@ -28,6 +47,7 @@ PhysicsLoopBack::~PhysicsLoopBack() { delete m_data->m_physicsClient; delete m_data->m_physicsServer; + delete m_data->m_commandProcessor; delete m_data; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 50925cb14..c5844bdfd 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -49,9 +49,8 @@ //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! btVector3 gLastPickPos(0, 0, 0); -bool gCloseToKuka=false; -bool gEnableRealTimeSimVR=false; -bool gCreateDefaultRobotAssets = false; + + int gInternalSimFlags = 0; bool gResetSimulation = 0; int gVRTrackingObjectUniqueId = -1; @@ -59,9 +58,9 @@ int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION; btTransform gVRTrackingObjectTr = btTransform::getIdentity(); -int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads -int gCreateObjectSimVR = -1; -int gEnableKukaControl = 0; + + + btVector3 gVRTeleportPos1(0,0,0); btQuaternion gVRTeleportOrn(0, 0, 0,1); @@ -1137,25 +1136,13 @@ struct PhysicsServerCommandProcessorInternalData bool m_allowRealTimeSimulation; - bool m_hasGround; + b3VRControllerEvents m_vrControllerEvents; btAlignedObjectArray m_keyboardEvents; - btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; - btMultiBody* m_gripperMultiBody; - btMultiBodyFixedConstraint* m_kukaGripperFixed; - btMultiBody* m_kukaGripperMultiBody; - btMultiBodyPoint2Point* m_kukaGripperRevolute1; - btMultiBodyPoint2Point* m_kukaGripperRevolute2; - - - int m_huskyId; - int m_KukaId; - int m_sphereId; - int m_gripperId; CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -1231,17 +1218,6 @@ struct PhysicsServerCommandProcessorInternalData PhysicsServerCommandProcessorInternalData() : m_allowRealTimeSimulation(false), - m_hasGround(false), - m_gripperRigidbodyFixed(0), - m_gripperMultiBody(0), - m_kukaGripperFixed(0), - m_kukaGripperMultiBody(0), - m_kukaGripperRevolute1(0), - m_kukaGripperRevolute2(0), - m_huskyId(-1), - m_KukaId(-1), - m_sphereId(-1), - m_gripperId(-1), m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.), @@ -4087,7 +4063,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) { //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk - gCreateDefaultRobotAssets = (clientCmd.m_physSimParamArgs.m_internalSimFlags & SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS); gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; } @@ -4111,11 +4086,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; } - if (clientCmd.m_updateFlags&SIM_PARAM_MAX_CMD_PER_1MS) - { - gMaxNumCmdPer1ms = clientCmd.m_physSimParamArgs.m_maxNumCmdPer1ms; - } - + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) { m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; @@ -6282,7 +6253,7 @@ btQuaternion gVRController2Orn(0,0,0,1); btScalar gVRGripper2Analog = 0; btScalar gVRGripperAnalog = 0; -bool gVRGripperClosed = false; + int gDroppedSimulationSteps = 0; @@ -6295,6 +6266,11 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime m_data->m_allowRealTimeSimulation = enableRealTimeSim; } +bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const +{ + return m_data->m_allowRealTimeSimulation; +} + void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents,const struct b3KeyboardEvent* keyEvents, int numKeyEvents) { m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents); @@ -6348,23 +6324,22 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const if (gVRTrackingObjectUniqueId>=0) { gVRTrackingObjectTr.setOrigin(bodyHandle->m_multiBody->getBaseWorldTransform().getOrigin()); + gVRTeleportPos1 = gVRTrackingObjectTr.getOrigin(); } if (gVRTrackingObjectFlag&VR_CAMERA_TRACK_OBJECT_ORIENTATION) { gVRTrackingObjectTr.setBasis(bodyHandle->m_multiBody->getBaseWorldTransform().getBasis()); + gVRTeleportOrn = gVRTrackingObjectTr.getRotation(); + } + } } if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) { - ///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later... - if (gCreateDefaultRobotAssets) - { - createDefaultRobotAssets(); - } - + int maxSteps = m_data->m_numSimulationSubSteps+3; if (m_data->m_numSimulationSubSteps) { @@ -6413,553 +6388,29 @@ void PhysicsServerCommandProcessor::resetSimulation() m_data->m_bodyHandles.exitHandles(); m_data->m_bodyHandles.initHandles(); - m_data->m_hasGround = false; - m_data->m_gripperRigidbodyFixed = 0; + + } -//todo: move this to Python/scripting (it is almost ready to be removed!) -void PhysicsServerCommandProcessor::createDefaultRobotAssets() -{ - static btAlignedObjectArray gBufferServerToClient; - gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - int bodyId = 0; - - if (gCreateObjectSimVR >= 0) - { - gCreateObjectSimVR = -1; - btMatrix3x3 mat(gVRGripperOrn); - btScalar spawnDistance = 0.1; - btVector3 spawnDir = mat.getColumn(0); - btVector3 shiftPos = spawnDir*spawnDistance; - btVector3 spawnPos = gVRGripperPos + shiftPos; - loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size(),0); - //loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - m_data->m_sphereId = bodyId; - InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId); - if (parentBody->m_multiBody) - { - parentBody->m_multiBody->setBaseVel(spawnDir * 5); - } - } - - if (!m_data->m_hasGround) - { - m_data->m_hasGround = true; - - loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); -// loadUrdf("quadruped/quadruped.urdf", btVector3(2, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - if (m_data->m_gripperRigidbodyFixed == 0) - { - int bodyId = 0; - - if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) - { - InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId); - if (parentBody->m_multiBody) - { - parentBody->m_multiBody->setHasSelfCollision(0); - btVector3 pivotInParent(0.2, 0, 0); - btMatrix3x3 frameInParent; - //frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); - frameInParent.setIdentity(); - btVector3 pivotInChild(0, 0, 0); - btMatrix3x3 frameInChild; - frameInChild.setIdentity(); - - m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); - m_data->m_gripperMultiBody = parentBody->m_multiBody; - if (m_data->m_gripperMultiBody->getNumLinks() > 2) - { - m_data->m_gripperMultiBody->setJointPos(0, 0); - m_data->m_gripperMultiBody->setJointPos(2, 0); - } - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); - } - } - } - - loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - m_data->m_KukaId = bodyId; - if (m_data->m_KukaId>=0) - { - InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId); - if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7) - { - btScalar q[7]; - q[0] = 0;// -SIMD_HALF_PI; - q[1] = 0; - q[2] = 0; - q[3] = SIMD_HALF_PI; - q[4] = 0; - q[5] = -SIMD_HALF_PI*0.66; - q[6] = 0; - - for (int i = 0; i < 7; i++) - { - kukaBody->m_multiBody->setJointPos(i, q[i]); - } - btAlignedObjectArray scratch_q; - btAlignedObjectArray scratch_m; - kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m); - int nLinks = kukaBody->m_multiBody->getNumLinks(); - scratch_q.resize(nLinks + 1); - scratch_m.resize(nLinks + 1); - kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m); - } - } -#if 1 - loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); -#endif - - // loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - -#if 1 - // Load one motor gripper for kuka - loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true,CUF_USE_SDF); - m_data->m_gripperId = bodyId + 1; - { - InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId); - - // Reset the default gripper motor maximum torque for damping to 0 - for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++) - { - if (supportsJointMotor(gripperBody->m_multiBody, i)) - { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr; - if (motor) - { - motor->setMaxAppliedImpulse(0); - } - } - } - } -#endif -#if 1 - for (int i = 0; i < 6; i++) - { - loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - } -#endif - //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Add slider joint for fingers - btVector3 pivotInParent1(-0.055, 0, 0.02); - btVector3 pivotInChild1(0, 0, 0); - btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0)); - btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0)); - btVector3 jointAxis1(1.0, 0, 0); - btVector3 pivotInParent2(0.055, 0, 0.02); - btVector3 pivotInChild2(0, 0, 0); - btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0)); - btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0)); - btVector3 jointAxis2(1.0, 0, 0); - - if (m_data->m_gripperId>=0) - { - InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId); - m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1); - m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0); - m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2); - m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0); - - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); - - } - - if (m_data->m_KukaId>=0) - { - InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId); - if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7) - { - if (m_data->m_gripperId>=0) - { - InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId); - - gripperBody->m_multiBody->setHasSelfCollision(0); - btVector3 pivotInParent(0, 0, 0.05); - btMatrix3x3 frameInParent; - frameInParent.setIdentity(); - btVector3 pivotInChild(0, 0, 0); - btMatrix3x3 frameInChild; - frameInChild.setIdentity(); - - m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); - m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody; - m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500); - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed); - } - } - } -#if 0 - - for (int i = 0; i < 10; i++) - { - loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - } - - loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); -#endif - btTransform objectLocalTr[] = { - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)), - btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)), - btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8)) - }; - - - btAlignedObjectArray objectWorldTr; - int numOb = sizeof(objectLocalTr) / sizeof(btTransform); - objectWorldTr.resize(numOb); - - btTransform tr; - tr.setIdentity(); - tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)); - tr.setOrigin(btVector3(1.0, -0.2, 0)); - - for (int i = 0; i < numOb; i++) - { - objectWorldTr[i] = tr*objectLocalTr[i]; - } - - // Table area - loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Shelf area - loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true, CUF_USE_SDF); - loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - // Chess area - loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - //loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - m_data->m_huskyId = bodyId; - - m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); - - } - - if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody) - { - InteralBodyData* childBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId); - // Add gripper controller - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr; - if (motor) - { - btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75; - motor->setPositionTarget(posTarget, .8); - motor->setVelocityTarget(0.0, .5); - motor->setMaxAppliedImpulse(1.0); - - } - } - - if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) - { - m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); - m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); - btScalar avg = 0.f; - - for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++) - { - if (supportsJointMotor(m_data->m_gripperMultiBody, i)) - { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr; - if (motor) - { - motor->setErp(0.2); - btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29; - btScalar maxPosTarget = 0.55; - - btScalar correction = 0.f; - - if (avg) - { - correction = m_data->m_gripperMultiBody->getJointPos(i) - avg; - } - - if (m_data->m_gripperMultiBody->getJointPos(i) < 0) - { - m_data->m_gripperMultiBody->setJointPos(i,0); - } - if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget) - { - m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget); - } - - if (avg) - { - motor->setPositionTarget(avg, 1); - } - else - { - motor->setPositionTarget(posTarget, 1); - } - motor->setVelocityTarget(0, 0.5); - btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(maxImp); - avg = m_data->m_gripperMultiBody->getJointPos(i); - - //motor->setRhsClamp(gRhsClamp); - } - } - } - } - - // Inverse kinematics for KUKA - if (m_data->m_KukaId>=0) - { - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(m_data->m_KukaId); - if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7) - { - btMultiBody* mb = bodyHandle->m_multiBody; - btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2(); - btScalar distanceThreshold = 1.3; - gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold)); - - int numDofs = bodyHandle->m_multiBody->getNumDofs(); - btAlignedObjectArray q_new; - btAlignedObjectArray q_current; - q_current.resize(numDofs); - for (int i = 0; i < numDofs; i++) - { - q_current[i] = bodyHandle->m_multiBody->getJointPos(i); - } - - q_new.resize(numDofs); - //sensible rest-pose - q_new[0] = 0;// -SIMD_HALF_PI; - q_new[1] = 0; - q_new[2] = 0; - q_new[3] = SIMD_HALF_PI; - q_new[4] = 0; - q_new[5] = -SIMD_HALF_PI*0.66; - q_new[6] = 0; - - if (gCloseToKuka && gEnableKukaControl) - { - double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0}; - - IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); - IKTrajectoryHelper* ikHelperPtr = 0; - - if (ikHelperPtrPtr) - { - ikHelperPtr = *ikHelperPtrPtr; - } - else - { - IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; - m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); - ikHelperPtr = tmpHelper; - } - - int endEffectorLinkIndex = 6; - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) - { - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - jacSize = jacobian_linear.size(); - // Set jacobian value - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - - btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); - for (int i = 0; i < numDofs; i++) - { - q_current[i] = bodyHandle->m_multiBody->getJointPos(i); - q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); - qdot[i + baseDofs] = 0; - nu[i+baseDofs] = 0; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3,numDofs); - btInverseDynamics::mat3x jac_r(3,numDofs); - tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); - tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,j); - jacobian_angular[i*numDofs+j] = jac_r(i,j); - } - } - } - } - - int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS; - - btVector3DoubleData endEffectorWorldPosition; - btVector3DoubleData endEffectorWorldOrientation; - btVector3DoubleData targetWorldPosition; - btVector3DoubleData targetWorldOrientation; - - btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); - btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); - btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); - - // Prescribed position and orientation - static btScalar time=0.f; - time+=0.01; - btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time)); - targetPos +=mb->getBasePos(); - btVector4 downOrn(0,1,0,0); - - // Controller orientation - btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w()); - - // Set position and orientation - endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); - endEffectorOri.serializeDouble(endEffectorWorldOrientation); - downOrn.serializeDouble(targetWorldOrientation); - //targetPos.serializeDouble(targetWorldPosition); - - gVRController2Pos.serializeDouble(targetWorldPosition); - - //controllerOrn.serializeDouble(targetWorldOrientation); - - if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - lower_limit[0] = -.967; - lower_limit[1] = -2.0; - lower_limit[2] = -2.96; - lower_limit[3] = 0.19; - lower_limit[4] = -2.96; - lower_limit[5] = -2.09; - lower_limit[6] = -3.05; - upper_limit[0] = .96; - upper_limit[1] = 2.0; - upper_limit[2] = 2.96; - upper_limit[3] = 2.29; - upper_limit[4] = 2.96; - upper_limit[5] = 2.09; - upper_limit[6] = 3.05; - joint_range[0] = 5.8; - joint_range[1] = 4; - joint_range[2] = 5.8; - joint_range[3] = 4; - joint_range[4] = 5.8; - joint_range[5] = 4; - joint_range[6] = 6; - rest_pose[0] = 0; - rest_pose[1] = 0; - rest_pose[2] = 0; - rest_pose[3] = SIMD_HALF_PI; - rest_pose[4] = 0; - rest_pose[5] = -SIMD_HALF_PI*0.66; - rest_pose[6] = 0; - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } - - ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk); - } - } - - //directly set the position of the links, only for debugging IK, don't use this method! -#if 0 - if (0) - { - for (int i=0;igetNumLinks();i++) - { - btScalar desiredPosition = q_new[i]; - mb->setJointPosMultiDof(i,&desiredPosition); - } - } else -#endif - { - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base - int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) - { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - if (motor) - { - btScalar desiredVelocity = 0.f; - btScalar desiredPosition = q_new[link]; - motor->setRhsClamp(gRhsClamp); - //printf("link %d: %f", link, q_new[link]); - motor->setVelocityTarget(desiredVelocity,1.0); - motor->setPositionTarget(desiredPosition,0.6); - btScalar maxImp = 1.0; - - motor->setMaxAppliedImpulse(maxImp); - numMotors++; - } - } - velIndex += mb->getLink(link).m_dofCount; - posIndex += mb->getLink(link).m_posVarCount; - } - } - } - } - - } -} void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/) { } + +const btVector3& PhysicsServerCommandProcessor::getVRTeleportPosition() const +{ + return gVRTeleportPos1; +} +void PhysicsServerCommandProcessor::setVRTeleportPosition(const btVector3& vrTeleportPos) +{ + gVRTeleportPos1 = vrTeleportPos; +} +const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() const +{ + return gVRTeleportOrn; +} +void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn) +{ + gVRTeleportOrn = vrTeleportOrn; +} diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 304740d49..2280242e6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -20,11 +20,6 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface struct PhysicsServerCommandProcessorInternalData* m_data; - - - //todo: move this to physics client side / Python - void createDefaultRobotAssets(); - void resetSimulation(); protected: @@ -101,9 +96,17 @@ public: virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents); virtual void enableRealTimeSimulation(bool enableRealTimeSim); + virtual bool isRealTimeSimulationEnabled() const; + void applyJointDamping(int bodyUniqueId); virtual void setTimeOut(double timeOutInSeconds); + + virtual const btVector3& getVRTeleportPosition() const; + virtual void setVRTeleportPosition(const btVector3& vrReleportPos); + + virtual const btQuaternion& getVRTeleportOrientation() const; + virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn); }; #endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 33dca816f..32477d96c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -4,9 +4,7 @@ #include "PhysicsServerExample.h" -#ifdef B3_USE_MIDI -#include "RtMidi.h" -#endif//B3_USE_MIDI + #include "PhysicsServerSharedMemory.h" #include "Bullet3Common/b3CommandLineArgs.h" @@ -24,37 +22,26 @@ //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! -extern btVector3 gLastPickPos; bool gEnablePicking=true; bool gEnableTeleporting=true; bool gEnableRendering= true; bool gEnableSyncPhysicsRendering= true; bool gEnableUpdateDebugDrawLines = true; + +//extern btVector3 gLastPickPos; btVector3 gVRTeleportPosLocal(0,0,0); btQuaternion gVRTeleportOrnLocal(0,0,0,1); -extern btVector3 gVRTeleportPos1; -extern btQuaternion gVRTeleportOrn; + btScalar gVRTeleportRotZ = 0; -extern btVector3 gVRGripperPos; -extern btQuaternion gVRGripperOrn; -extern btVector3 gVRController2Pos; -extern btQuaternion gVRController2Orn; -extern btScalar gVRGripperAnalog; -extern btScalar gVRGripper2Analog; -extern bool gCloseToKuka; -extern bool gEnableRealTimeSimVR; -extern bool gCreateDefaultRobotAssets; extern int gInternalSimFlags; -extern int gCreateObjectSimVR; extern bool gResetSimulation; -extern int gEnableKukaControl; int gGraspingController = -1; extern btScalar simTimeScalingFactor; bool gBatchUserDebugLines = true; -extern bool gVRGripperClosed; + const char* startFileNameVR = "0_VRDemoSettings.txt"; @@ -82,70 +69,20 @@ static void loadCurrentSettingsVR(b3CommandLineArgs& args) }; //remember the settings (you don't want to re-tune again and again...) -static void saveCurrentSettingsVR() + + +static void saveCurrentSettingsVR(const btVector3& VRTeleportPos1) { FILE* f = fopen(startFileNameVR, "w"); if (f) { - fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]); - fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]); - fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]); + fprintf(f, "--camPosX= %f\n", VRTeleportPos1[0]); + fprintf(f, "--camPosY= %f\n", VRTeleportPos1[1]); + fprintf(f, "--camPosZ= %f\n", VRTeleportPos1[2]); fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ); fclose(f); } }; - -#if B3_USE_MIDI - - - -static float getParamf(float rangeMin, float rangeMax, int midiVal) -{ - float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.)); - return v; -} -void midiCallback(double deltatime, std::vector< unsigned char > *message, void *userData) -{ - unsigned int nBytes = message->size(); - for (unsigned int i = 0; i 0) - std::cout << "stamp = " << deltatime << std::endl; - - if (nBytes > 2) - { - - if (message->at(0) == 176) - { - if (message->at(1) == 16) - { - gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2)); - gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); - saveCurrentSettingsVR(); -// b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ); - } - - if (message->at(1) == 32) - { - gCreateDefaultRobotAssets = 1; - } - - for (int i = 0; i < 3; i++) - { - if (message->at(1) == i) - { - gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2)); - saveCurrentSettingsVR(); -// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPosLocal[i]); - - } - } - } - } -} - -#endif //B3_USE_MIDI - bool gDebugRenderToggle = false; void MotionThreadFunc(void* userPtr,void* lsMemory); void* MotionlsMemoryFunc(); @@ -291,7 +228,6 @@ struct MotionThreadLocalStorage float clampedDeltaTime = 0.2; -extern int gMaxNumCmdPer1ms; void MotionThreadFunc(void* userPtr,void* lsMemory) @@ -325,16 +261,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) { - if (gMaxNumCmdPer1ms>0) - { - if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms) - { - BT_PROFILE("usleep(10)"); - b3Clock::usleep(10); - numCmdSinceSleep1ms = 0; - sleepClock.reset(); - } - } + if (sleepClock.getTimeMilliseconds()>1) { sleepClock.reset(); @@ -1229,9 +1156,7 @@ class PhysicsServerExample : public SharedMemoryCommon MotionArgs m_args[MAX_MOTION_NUM_THREADS]; MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper; bool m_wantsShutdown; -#ifdef B3_USE_MIDI - RtMidiIn* m_midi; -#endif + bool m_isConnected; btClock m_clock; bool m_replay; @@ -1243,7 +1168,7 @@ class PhysicsServerExample : public SharedMemoryCommon public: - PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0); + PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem=0, int options=0); virtual ~PhysicsServerExample(); @@ -1434,43 +1359,54 @@ public: if (window->isModifierKeyPressed(B3G_SHIFT)) shift=0.01; + btVector3 VRTeleportPos =this->m_physicsServer.getVRTeleportPosition(); + if (key=='w' && state) { - gVRTeleportPos1[0]+=shift; - saveCurrentSettingsVR(); + VRTeleportPos[0]+=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); } if (key=='s' && state) { - gVRTeleportPos1[0]-=shift; - saveCurrentSettingsVR(); + VRTeleportPos[0]-=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); } if (key=='a' && state) { - gVRTeleportPos1[1]-=shift; - saveCurrentSettingsVR(); + VRTeleportPos[1]-=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); } if (key=='d' && state) { - gVRTeleportPos1[1]+=shift; - saveCurrentSettingsVR(); + VRTeleportPos[1]+=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); } if (key=='q' && state) { - gVRTeleportPos1[2]+=shift; - saveCurrentSettingsVR(); + VRTeleportPos[2]+=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); } if (key=='e' && state) { - gVRTeleportPos1[2]-=shift; - saveCurrentSettingsVR(); + VRTeleportPos[2]-=shift; + m_physicsServer.setVRTeleportPosition(VRTeleportPos); + saveCurrentSettingsVR(VRTeleportPos); } if (key=='z' && state) { gVRTeleportRotZ+=shift; - gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); - saveCurrentSettingsVR(); + btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); + m_physicsServer.setVRTeleportOrientation(VRTeleportOrn); + saveCurrentSettingsVR(VRTeleportPos); } + + return false; } @@ -1490,33 +1426,33 @@ public: setSharedMemoryKey(shmemKey); } - if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0])) + btVector3 vrTeleportPos = m_physicsServer.getVRTeleportPosition(); + + if (args.GetCmdLineArgument("camPosX", vrTeleportPos[0])) { - printf("camPosX=%f\n", gVRTeleportPos1[0]); + printf("camPosX=%f\n", vrTeleportPos[0]); } - if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1])) + if (args.GetCmdLineArgument("camPosY", vrTeleportPos[1])) { - printf("camPosY=%f\n", gVRTeleportPos1[1]); + printf("camPosY=%f\n", vrTeleportPos[1]); } - if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2])) + if (args.GetCmdLineArgument("camPosZ", vrTeleportPos[2])) { - printf("camPosZ=%f\n", gVRTeleportPos1[2]); + printf("camPosZ=%f\n", vrTeleportPos[2]); } + m_physicsServer.setVRTeleportPosition(vrTeleportPos); + float camRotZ = 0.f; if (args.GetCmdLineArgument("camRotZ", camRotZ)) { printf("camRotZ = %f\n", camRotZ); btQuaternion ornZ(btVector3(0, 0, 1), camRotZ); - gVRTeleportOrn = ornZ; + m_physicsServer.setVRTeleportOrientation(ornZ); } - if (args.CheckCmdLineFlag("robotassets")) - { - gCreateDefaultRobotAssets = true; - } if (args.CheckCmdLineFlag("realtimesimulation")) { @@ -1524,53 +1460,17 @@ public: m_physicsServer.enableRealTimeSimulation(true); } - if (args.CheckCmdLineFlag("norobotassets")) - { - gCreateDefaultRobotAssets = false; - } + } }; -#ifdef B3_USE_MIDI -static bool chooseMidiPort(RtMidiIn *rtmidi) -{ - /* - std::cout << "\nWould you like to open a virtual input port? [y/N] "; - - std::string keyHit; - std::getline( std::cin, keyHit ); - if ( keyHit == "y" ) { - rtmidi->openVirtualPort(); - return true; - } - */ - - std::string portName; - unsigned int i = 0, nPorts = rtmidi->getPortCount(); - if (nPorts == 0) { - std::cout << "No midi input ports available!" << std::endl; - return false; - } - - if (nPorts > 0) { - std::cout << "\nOpening midi input port " << rtmidi->getPortName() << std::endl; - } - - // std::getline( std::cin, keyHit ); // used to clear out stdin - rtmidi->openPort(i); - - return true; -} -#endif //B3_USE_MIDI - - -PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options) +PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper,CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int options) :SharedMemoryCommon(helper), -m_physicsServer(sharedMem), +m_physicsServer(commandProcessorCreator,sharedMem,0), m_wantsShutdown(false), m_isConnected(false), m_replay(false) @@ -1579,14 +1479,7 @@ m_replay(false) ,m_tinyVrGui(0) #endif { -#ifdef B3_USE_MIDI - m_midi = new RtMidiIn(); - chooseMidiPort(m_midi); - m_midi->setCallback(&midiCallback); - // Don't ignore sysex, timing, or active sensing messages. - m_midi->ignoreTypes(false, false, false); -#endif m_multiThreadedHelper = helper; // b3Printf("Started PhysicsServer\n"); } @@ -1595,10 +1488,7 @@ m_replay(false) PhysicsServerExample::~PhysicsServerExample() { -#ifdef B3_USE_MIDI - delete m_midi; - m_midi = 0; -#endif + #ifdef BT_ENABLE_VR delete m_tinyVrGui; #endif @@ -2007,7 +1897,6 @@ extern int gDroppedSimulationSteps; extern int gNumSteps; extern double gDtInSec; extern double gSubStep; -extern int gVRTrackingObjectUniqueId; extern btTransform gVRTrackingObjectTr; @@ -2206,28 +2095,15 @@ void PhysicsServerExample::drawUserDebugLines() void PhysicsServerExample::renderScene() { btTransform vrTrans; - //gVRTeleportPos1 = gVRTeleportPosLocal; - //gVRTeleportOrn = gVRTeleportOrnLocal; - ///little VR test to follow/drive Husky vehicle - if (gVRTrackingObjectUniqueId >= 0) - { - btTransform vrTrans; - vrTrans.setOrigin(gVRTeleportPosLocal); - vrTrans.setRotation(gVRTeleportOrnLocal); - - vrTrans = vrTrans * gVRTrackingObjectTr; - gVRTeleportPos1 = vrTrans.getOrigin(); - gVRTeleportOrn = vrTrans.getRotation(); - } B3_PROFILE("PhysicsServerExample::RenderScene"); drawUserDebugLines(); - if (gEnableRealTimeSimVR) + if (m_physicsServer.isRealTimeSimulationEnabled()) { static int frameCount=0; @@ -2285,8 +2161,10 @@ void PhysicsServerExample::renderScene() { b3Transform tr;tr.setIdentity(); - tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2])); - tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3])); + btVector3 VRController2Pos = m_physicsServer.getVRTeleportPosition(); + btQuaternion VRController2Orn = m_physicsServer.getVRTeleportOrientation(); + tr.setOrigin(b3MakeVector3(VRController2Pos[0],VRController2Pos[1],VRController2Pos[2])); + tr.setRotation(b3Quaternion(VRController2Orn[0],VRController2Orn[1],VRController2Orn[2],VRController2Orn[3])); tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0)); b3Scalar dt = 0.01; m_tinyVrGui->clearTextArea(); @@ -2304,8 +2182,8 @@ void PhysicsServerExample::renderScene() btTransform tr2a, tr2; tr2a.setIdentity(); tr2.setIdentity(); - tr2.setOrigin(gVRTeleportPos1); - tr2a.setRotation(gVRTeleportOrn); + tr2.setOrigin(m_physicsServer.getVRTeleportPosition()); + tr2a.setRotation(m_physicsServer.getVRTeleportOrientation()); btTransform trTotal = tr2*tr2a; btTransform trInv = trTotal.inverse(); @@ -2368,9 +2246,8 @@ void PhysicsServerExample::renderScene() if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { - if (!gEnableRealTimeSimVR) + if (!m_physicsServer.isRealTimeSimulationEnabled()) { - gEnableRealTimeSimVR = true; m_physicsServer.enableRealTimeSimulation(1); } } @@ -2470,35 +2347,6 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y) } -extern int gSharedMemoryKey; - - -class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options) -{ - - MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper); - - - PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper, - options.m_sharedMem, - options.m_option); - - if (gSharedMemoryKey>=0) - { - example->setSharedMemoryKey(gSharedMemoryKey); - } - if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING) - { - example->enableCommandLogging(); - } - if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG) - { - example->replayFromLogFile(); - } - return example; - -} - void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]) @@ -2511,7 +2359,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt if (gGraspingController < 0) { gGraspingController = controllerId; - gEnableKukaControl = true; } btTransform trLocal; @@ -2530,8 +2377,8 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt - tr2.setOrigin(gVRTeleportPos1); - tr2a.setRotation(gVRTeleportOrn); + tr2.setOrigin(m_physicsServer.getVRTeleportPosition()); + tr2a.setRotation(m_physicsServer.getVRTeleportOrientation()); btTransform trTotal = tr2*tr2a*trOrg*trLocal; @@ -2541,8 +2388,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt { if (button == 1 && state == 0) { - //gResetSimulation = true; - gVRTeleportPos1 = gLastPickPos; + } } else { @@ -2583,7 +2429,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt if (controllerId == gGraspingController) { - gCreateObjectSimVR = 1; } else { @@ -2599,7 +2444,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt if (controllerId == gGraspingController && (button == 33)) { - gVRGripperClosed =(state!=0); + } else { @@ -2669,25 +2514,18 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ - tr2.setOrigin(gVRTeleportPos1); - tr2a.setRotation(gVRTeleportOrn); + tr2.setOrigin(m_physicsServer.getVRTeleportPosition()); + tr2a.setRotation(m_physicsServer.getVRTeleportOrientation()); btTransform trTotal = tr2*tr2a*trOrg*trLocal; if (controllerId == gGraspingController) { - gVRGripperAnalog = analogAxis; - gVRGripperPos = trTotal.getOrigin(); - gVRGripperOrn = trTotal.getRotation(); } else { - gVRGripper2Analog = analogAxis; - gVRController2Pos = trTotal.getOrigin(); - gVRController2Orn = trTotal.getRotation(); - m_args[0].m_vrControllerPos[controllerId] = trTotal.getOrigin(); m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation(); } @@ -2730,8 +2568,8 @@ void PhysicsServerExample::vrHMDMoveCallback(int controllerId, float pos[4], flo tr2a.setIdentity(); btTransform tr2; tr2.setIdentity(); - tr2.setOrigin(gVRTeleportPos1); - tr2a.setRotation(gVRTeleportOrn); + tr2.setOrigin(m_physicsServer.getVRTeleportPosition()); + tr2a.setRotation(m_physicsServer.getVRTeleportOrientation()); btTransform trTotal = tr2*tr2a*trOrg*trLocal; @@ -2772,8 +2610,8 @@ void PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float tr2a.setIdentity(); btTransform tr2; tr2.setIdentity(); - tr2.setOrigin(gVRTeleportPos1); - tr2a.setRotation(gVRTeleportOrn); + tr2.setOrigin(m_physicsServer.getVRTeleportPosition()); + tr2a.setRotation(m_physicsServer.getVRTeleportOrientation()); btTransform trTotal = tr2*tr2a*trOrg*trLocal; m_args[0].m_csGUI->lock(); @@ -2790,4 +2628,36 @@ void PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float m_args[0].m_csGUI->unlock(); } -B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) + +extern int gSharedMemoryKey; + + +class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options) +{ + + MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper); + + + PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper, + options.m_commandProcessorCreation, + options.m_sharedMem, + options.m_option); + + if (gSharedMemoryKey>=0) + { + example->setSharedMemoryKey(gSharedMemoryKey); + } + if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING) + { + example->enableCommandLogging(); + } + if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG) + { + example->replayFromLogFile(); + } + return example; + +} + + + diff --git a/examples/SharedMemory/PhysicsServerExample.h b/examples/SharedMemory/PhysicsServerExample.h index 8df83bc98..0f983c843 100644 --- a/examples/SharedMemory/PhysicsServerExample.h +++ b/examples/SharedMemory/PhysicsServerExample.h @@ -8,7 +8,9 @@ enum PhysicsServerOptions PHYSICS_SERVER_USE_RTC_CLOCK = 4, }; -class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options); +///Don't use PhysicsServerCreateFuncInternal directly +///Use PhysicsServerCreateFuncBullet2 instead, or initialize options.m_commandProcessor +class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options); #endif //PHYSICS_SERVER_EXAMPLE_H diff --git a/examples/SharedMemory/PhysicsServerExampleBullet2.cpp b/examples/SharedMemory/PhysicsServerExampleBullet2.cpp new file mode 100644 index 000000000..fbd7cd9d2 --- /dev/null +++ b/examples/SharedMemory/PhysicsServerExampleBullet2.cpp @@ -0,0 +1,35 @@ + +#include "PhysicsServerExampleBullet2.h" +#include "PhysicsServerExample.h" +#include "PhysicsServerCommandProcessor.h" +#include "../CommonInterfaces/CommonExampleInterface.h" + +struct Bullet2CommandProcessorCreation : public CommandProcessorCreationInterface +{ + virtual class CommandProcessorInterface* createCommandProcessor() + { + PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor; + return proc; + } + + virtual void deleteCommandProcessor(CommandProcessorInterface* proc) + { + delete proc; + } +}; + + +static Bullet2CommandProcessorCreation sBullet2CommandCreator; + +CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options) +{ + options.m_commandProcessorCreation = &sBullet2CommandCreator; + + CommonExampleInterface* example = PhysicsServerCreateFuncInternal(options); + return example; + +} + +B3_STANDALONE_EXAMPLE(PhysicsServerCreateFuncBullet2) + + diff --git a/examples/SharedMemory/PhysicsServerExampleBullet2.h b/examples/SharedMemory/PhysicsServerExampleBullet2.h new file mode 100644 index 000000000..5281caa2f --- /dev/null +++ b/examples/SharedMemory/PhysicsServerExampleBullet2.h @@ -0,0 +1,9 @@ + +#ifndef PHYSICS_SERVER_EXAMPLE_BULLET_2_H +#define PHYSICS_SERVER_EXAMPLE_BULLET_2_H + + + +class CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options); + +#endif //PHYSICS_SERVER_EXAMPLE_BULLET_2_H diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 3e9e42dc1..8eea09002 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -3,7 +3,7 @@ #include "Win32SharedMemory.h" #include "../CommonInterfaces/CommonRenderInterface.h" - +#include "../CommonInterfaces/CommonExampleInterface.h" #include "btBulletDynamicsCommon.h" #include "LinearMath/btTransform.h" @@ -31,7 +31,8 @@ struct PhysicsServerSharedMemoryInternalData bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS]; bool m_verboseOutput; CommandProcessorInterface* m_commandProcessor; - + CommandProcessorCreationInterface* m_commandProcessorCreator; + PhysicsServerSharedMemoryInternalData() :m_sharedMemory(0), m_ownsSharedMemory(false), @@ -64,9 +65,10 @@ struct PhysicsServerSharedMemoryInternalData }; -PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem) +PhysicsServerSharedMemory::PhysicsServerSharedMemory(CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int bla) { m_data = new PhysicsServerSharedMemoryInternalData(); + m_data->m_commandProcessorCreator = commandProcessorCreator; if (sharedMem) { m_data->m_sharedMemory = sharedMem; @@ -81,17 +83,13 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar m_data->m_ownsSharedMemory = true; } - m_data->m_commandProcessor = new PhysicsServerCommandProcessor; + m_data->m_commandProcessor = commandProcessorCreator->createCommandProcessor(); } PhysicsServerSharedMemory::~PhysicsServerSharedMemory() { - - //m_data->m_commandProcessor->deleteDynamicsWorld(); - delete m_data->m_commandProcessor; - if (m_data->m_sharedMemory) { if (m_data->m_verboseOutput) @@ -105,7 +103,7 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory() m_data->m_sharedMemory = 0; } - + m_data->m_commandProcessorCreator->deleteCommandProcessor(m_data->m_commandProcessor); delete m_data; } @@ -238,6 +236,11 @@ void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim) m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim); } +bool PhysicsServerSharedMemory::isRealTimeSimulationEnabled() const +{ + return m_data->m_commandProcessor->isRealTimeSimulationEnabled(); +} + void PhysicsServerSharedMemory::processClientCommands() @@ -315,3 +318,24 @@ void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName) { m_data->m_commandProcessor->replayFromLogFile(fileName); } + +const btVector3& PhysicsServerSharedMemory::getVRTeleportPosition() const +{ + return m_data->m_commandProcessor->getVRTeleportPosition(); +} +void PhysicsServerSharedMemory::setVRTeleportPosition(const btVector3& vrTeleportPos) +{ + m_data->m_commandProcessor->setVRTeleportPosition(vrTeleportPos); +} + +const btQuaternion& PhysicsServerSharedMemory::getVRTeleportOrientation() const +{ + return m_data->m_commandProcessor->getVRTeleportOrientation(); + +} +void PhysicsServerSharedMemory::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn) +{ + m_data->m_commandProcessor->setVRTeleportOrientation(vrTeleportOrn); +} + + diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index 68fa376bb..0fcd534b4 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -2,7 +2,8 @@ #define PHYSICS_SERVER_SHARED_MEMORY_H #include "PhysicsServer.h" - +#include "LinearMath/btQuaternion.h" + class PhysicsServerSharedMemory : public PhysicsServer { struct PhysicsServerSharedMemoryInternalData* m_data; @@ -14,7 +15,7 @@ protected: public: - PhysicsServerSharedMemory(class SharedMemoryInterface* sharedMem=0); + PhysicsServerSharedMemory(struct CommandProcessorCreationInterface* commandProcessorCreator, class SharedMemoryInterface* sharedMem, int bla); virtual ~PhysicsServerSharedMemory(); virtual void setSharedMemoryKey(int key); @@ -29,6 +30,7 @@ public: virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents); virtual void enableRealTimeSimulation(bool enableRealTimeSim); + virtual bool isRealTimeSimulationEnabled() const; //bool supportsJointMotor(class btMultiBody* body, int linkIndex); @@ -38,6 +40,14 @@ public: virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); virtual void removePickingConstraint(); + virtual const btVector3& getVRTeleportPosition() const; + virtual void setVRTeleportPosition(const btVector3& vrReleportPos); + + virtual const btQuaternion& getVRTeleportOrientation() const; + virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn); + + + //for physicsDebugDraw and renderScene are mainly for debugging purposes //and for physics visualization. The idea is that physicsDebugDraw can also send wireframe //to a physics client, over shared memory diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 8c259c802..633441c95 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -4,7 +4,9 @@ #include "PhysicsClientSharedMemory.h" #include"../ExampleBrowser/InProcessExampleBrowser.h" -#include "PhysicsServerExample.h" + +#include "PhysicsServerExampleBullet2.h" + #include "../CommonInterfaces/CommonExampleInterface.h" #include "InProcessMemory.h" @@ -142,7 +144,7 @@ public: CommonExampleOptions options(guiHelper); options.m_sharedMem = m_sharedMem; - m_physicsServerExample = PhysicsServerCreateFunc(options); + m_physicsServerExample = PhysicsServerCreateFuncBullet2(options); m_physicsServerExample ->initPhysics(); m_physicsServerExample ->resetCamera(); setSharedMemoryInterface(m_sharedMem); diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index e6cf9e25f..9bf4bd83f 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -14,7 +14,8 @@ subject to the following restrictions: */ -#include "PhysicsServerExample.h" + +#include "PhysicsServerExampleBullet2.h" #include "Bullet3Common/b3CommandLineArgs.h" @@ -77,7 +78,7 @@ int main(int argc, char* argv[]) // options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING; // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG; - example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); + example = (SharedMemoryCommon*)PhysicsServerCreateFuncBullet2(options); example->initPhysics(); diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index ff2ab709f..b7d3ab9e7 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -23,6 +23,7 @@ myfiles = "PhysicsClientSharedMemory.cpp", "PhysicsClientExample.cpp", "PhysicsServerExample.cpp", + "PhysicsServerExampleBullet2.cpp", "PhysicsServerSharedMemory.cpp", "PhysicsServerSharedMemory.h", "PhysicsServer.cpp", diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index f828ad7ea..c2f9fd49f 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -33,6 +33,7 @@ SET(pybullet_SRCS ../../examples/SharedMemory/PhysicsServer.cpp ../../examples/SharedMemory/PhysicsServer.h ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp ../../examples/SharedMemory/PhysicsServerSharedMemory.h diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 7892d3da2..20b41a49f 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -106,6 +106,7 @@ if not _OPTIONS["no-enet"] then "../../examples/SharedMemory/PhysicsServer.cpp", "../../examples/SharedMemory/PhysicsServer.h", "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp", "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.h", diff --git a/setup.py b/setup.py index 1b1d89436..9de213679 100644 --- a/setup.py +++ b/setup.py @@ -54,6 +54,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/PhysicsClient.cpp"]\ +["examples/SharedMemory/PhysicsServer.cpp"]\ +["examples/SharedMemory/PhysicsServerExample.cpp"]\ ++["examples/SharedMemory/PhysicsServerExampleBullet2.cpp"]\ +["examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp"]\ +["examples/SharedMemory/PhysicsServerSharedMemory.cpp"]\ +["examples/SharedMemory/PhysicsDirect.cpp"]\ @@ -417,7 +418,7 @@ else: setup( name = 'pybullet', - version='1.0.8', + version='1.0.9', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 6f8c6df9d..282cc31fc 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -360,6 +360,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/SharedMemory/PhysicsServer.cpp", "../../examples/SharedMemory/PhysicsServer.h", "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp", "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", "../../examples/SharedMemory/PhysicsServerSharedMemory.h",