From a4f1e348994e7eea987c9572745232e37a37a67c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Feb 2017 15:34:11 -0800 Subject: [PATCH] expose timeout in pybullet/shared memory API add RobotSimulator, a C++ API similar to pybullet. (work-in-progress, only part of API implemeted) --- CMakeLists.txt | 5 +- build3/premake4.lua | 1 + ...visual_studio_vr_pybullet_double_cmake.bat | 4 + examples/CMakeLists.txt | 2 +- examples/RobotSimulator/CMakeLists.txt | 159 ++++ examples/RobotSimulator/MinitaurSetup.cpp | 96 +++ examples/RobotSimulator/MinitaurSetup.h | 20 + .../RobotSimulator/RobotSimulatorMain.cpp | 66 ++ .../b3RobotSimulatorClientAPI.cpp | 776 ++++++++++++++++++ .../b3RobotSimulatorClientAPI.h | 191 +++++ examples/RobotSimulator/premake4.lua | 158 ++++ examples/SharedMemory/CMakeLists.txt | 40 +- examples/SharedMemory/PhysicsClient.h | 3 + examples/SharedMemory/PhysicsClientC_API.cpp | 27 +- examples/SharedMemory/PhysicsClientC_API.h | 4 + .../PhysicsClientSharedMemory.cpp | 13 +- .../SharedMemory/PhysicsClientSharedMemory.h | 3 + examples/SharedMemory/PhysicsClientTCP.cpp | 13 +- examples/SharedMemory/PhysicsClientTCP.h | 2 + examples/SharedMemory/PhysicsClientUDP.cpp | 16 +- examples/SharedMemory/PhysicsClientUDP.h | 1 + .../PhysicsCommandProcessorInterface.h | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 28 +- examples/SharedMemory/PhysicsDirect.h | 2 + examples/SharedMemory/PhysicsLoopBack.cpp | 10 + examples/SharedMemory/PhysicsLoopBack.h | 2 + .../PhysicsServerCommandProcessor.cpp | 4 + .../PhysicsServerCommandProcessor.h | 2 + .../SharedMemoryCommandProcessor.cpp | 4 +- .../SharedMemoryCommandProcessor.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 13 +- examples/SharedMemory/premake4.lua | 8 +- examples/pybullet/CMakeLists.txt | 2 +- examples/pybullet/pybullet.c | 37 +- 34 files changed, 1654 insertions(+), 62 deletions(-) create mode 100644 build_visual_studio_vr_pybullet_double_cmake.bat create mode 100644 examples/RobotSimulator/CMakeLists.txt create mode 100644 examples/RobotSimulator/MinitaurSetup.cpp create mode 100644 examples/RobotSimulator/MinitaurSetup.h create mode 100644 examples/RobotSimulator/RobotSimulatorMain.cpp create mode 100644 examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp create mode 100644 examples/RobotSimulator/b3RobotSimulatorClientAPI.h create mode 100644 examples/RobotSimulator/premake4.lua diff --git a/CMakeLists.txt b/CMakeLists.txt index d3fb18d3c..05a7897e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -210,7 +210,10 @@ ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF) - + +OPTION(BUILD_ENET "Set when you want to build apps with enet UDP networking support" ON) +OPTION(BUILD_CLSOCKET "Set when you want to build apps with enet TCP networking support" ON) + IF(BUILD_PYBULLET) diff --git a/build3/premake4.lua b/build3/premake4.lua index 9d4efd5ab..57ea7412e 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -253,6 +253,7 @@ end if not _OPTIONS["no-demos"] then include "../examples/ExampleBrowser" + include "../examples/RobotSimulator" include "../examples/OpenGLWindow" include "../examples/ThirdPartyLibs/Gwen" include "../examples/HelloWorld" diff --git a/build_visual_studio_vr_pybullet_double_cmake.bat b/build_visual_studio_vr_pybullet_double_cmake.bat new file mode 100644 index 000000000..2d3cd1a4b --- /dev/null +++ b/build_visual_studio_vr_pybullet_double_cmake.bat @@ -0,0 +1,4 @@ +mkdir build_cmake +cd build_cmake +cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.2\include -DPYTHON_LIBRARY=c:\python-3.5.2\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.2\libs\python35_d.lib -G "Visual Studio 14 2015" .. +start . diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 4c21d89d8..8f2f9503d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,6 +1,6 @@ SUBDIRS( HelloWorld BasicDemo ) IF(BUILD_BULLET3) - SUBDIRS( ExampleBrowser SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow ) + SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow ) ENDIF() IF(BUILD_PYBULLET) SUBDIRS(pybullet) diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt new file mode 100644 index 000000000..0dbf71053 --- /dev/null +++ b/examples/RobotSimulator/CMakeLists.txt @@ -0,0 +1,159 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src + ${BULLET_PHYSICS_SOURCE_DIR}/examples + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src + ) + + +SET(RobotSimulator_SRCS + RobotSimulatorMain.cpp + b3RobotSimulatorClientAPI.cpp + b3RobotSimulatorClientAPI.h + MinitaurSetup.cpp + MinitaurSetup.h + ../../examples/SharedMemory/IKTrajectoryHelper.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.h + ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp + ../../examples/SharedMemory/InProcessMemory.cpp + ../../examples/SharedMemory/PhysicsClient.cpp + ../../examples/SharedMemory/PhysicsClient.h + ../../examples/SharedMemory/PhysicsServer.cpp + ../../examples/SharedMemory/PhysicsServer.h + ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.h + ../../examples/SharedMemory/PhysicsDirect.cpp + ../../examples/SharedMemory/PhysicsDirect.h + ../../examples/SharedMemory/PhysicsDirectC_API.cpp + ../../examples/SharedMemory/PhysicsDirectC_API.h + ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp + ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.h + ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h + ../../examples/SharedMemory/PhysicsClientC_API.cpp + ../../examples/SharedMemory/PhysicsClientC_API.h + ../../examples/SharedMemory/Win32SharedMemory.cpp + ../../examples/SharedMemory/Win32SharedMemory.h + ../../examples/SharedMemory/PosixSharedMemory.cpp + ../../examples/SharedMemory/PosixSharedMemory.h + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Utils/b3ResourcePath.h + ../../examples/Utils/RobotLoggingUtil.cpp + ../../examples/Utils/RobotLoggingUtil.h + ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp + ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/MultiThreading/b3PosixThreadSupport.cpp + ../../examples/MultiThreading/b3Win32ThreadSupport.cpp + ../../examples/MultiThreading/b3ThreadSupportInterface.cpp + +) + +IF(BUILD_CLSOCKET) + ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET) +ENDIF(BUILD_CLSOCKET) + +IF(WIN32) + LINK_LIBRARIES( + ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + IF(BUILD_ENET) + ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET) + ENDIF(BUILD_ENET) + IF(BUILD_CLSOCKET) + ADD_DEFINITIONS(-DWIN32) + ENDIF(BUILD_CLSOCKET) + +ELSE(WIN32) + IF(BUILD_ENET) + ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET) + ENDIF(BUILD_ENET) + + IF(BUILD_CLSOCKET) + IF(APPLE) + ADD_DEFINITIONS(-D_DARWIN) + ELSE() + ADD_DEFINITIONS(-D_LINUX) + ENDIF() + ENDIF(BUILD_CLSOCKET) +ENDIF(WIN32) + + +IF(BUILD_ENET) + set(RobotSimulator_SRCS ${RobotSimulator_SRCS} + ../../examples/SharedMemory/PhysicsClientUDP.cpp + ../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp + ../../examples/SharedMemory/PhysicsClientUDP.h + ../../examples/SharedMemory/PhysicsClientUDP_C_API.h + ../../examples/ThirdPartyLibs/enet/win32.c + ../../examples/ThirdPartyLibs/enet/unix.c + ../../examples/ThirdPartyLibs/enet/callbacks.c + ../../examples/ThirdPartyLibs/enet/compress.c + ../../examples/ThirdPartyLibs/enet/host.c + ../../examples/ThirdPartyLibs/enet/list.c + ../../examples/ThirdPartyLibs/enet/packet.c + ../../examples/ThirdPartyLibs/enet/peer.c + ../../examples/ThirdPartyLibs/enet/protocol.c + ) +ENDIF(BUILD_ENET) + +IF(BUILD_CLSOCKET) + set(RobotSimulator_SRCS ${RobotSimulator_SRCS} + ../../examples/SharedMemory/PhysicsClientTCP.cpp + ../../examples/SharedMemory/PhysicsClientTCP.h + ../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp + ../../examples/SharedMemory/PhysicsClientTCP_C_API.h + ../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp + ../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp + ../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp + ) +ENDIF() + +ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS}) + + +SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d") + + +IF(WIN32) + IF(BUILD_ENET OR BUILD_CLSOCKET) + TARGET_LINK_LIBRARIES(App_RobotSimulator ws2_32 ) + ENDIF(BUILD_ENET OR BUILD_CLSOCKET) +ENDIF(WIN32) + + + + +TARGET_LINK_LIBRARIES(App_RobotSimulator BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common) + + diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp new file mode 100644 index 000000000..edd233789 --- /dev/null +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -0,0 +1,96 @@ +#include "MinitaurSetup.h" +#include "b3RobotSimulatorClientAPI.h" + +#include "Bullet3Common/b3HashMap.h" + +struct MinitaurSetupInternalData +{ + int m_quadrupedUniqueId; + + MinitaurSetupInternalData() + :m_quadrupedUniqueId(-1) + { + } + + b3HashMap m_jointNameToId; + +}; + + +MinitaurSetup::MinitaurSetup() +{ + m_data = new MinitaurSetupInternalData(); +} + +MinitaurSetup::~MinitaurSetup() +{ + delete m_data; +} + +void MinitaurSetup::resetPose() +{ +#if 0 + def resetPose(self): + #right front leg + self.disableAllMotors() + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_front_rightR_joint', 1.57) + self.setMotorAngleByName('motor_front_rightL_joint',-1.57) + + #left front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_front_leftR_joint', 1.57) + self.setMotorAngleByName('motor_front_leftL_joint',-1.57) + + #right back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_back_rightR_joint', 1.57) + self.setMotorAngleByName('motor_back_rightL_joint',-1.57) + + #left back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_back_leftR_joint', 1.57) + self.setMotorAngleByName('motor_back_leftL_joint',-1.57) +#endif +} + +int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) +{ + + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition = startPos; + args.m_startOrientation = startOrn; + + m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args); + + int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); + for (int i=0;igetJointInfo(m_data->m_quadrupedUniqueId,i,&jointInfo); + if (jointInfo.m_jointName) + { + m_data->m_jointNameToId.insert(jointInfo.m_jointName,i); + } + } + + resetPose(); + + return m_data->m_quadrupedUniqueId; +} \ No newline at end of file diff --git a/examples/RobotSimulator/MinitaurSetup.h b/examples/RobotSimulator/MinitaurSetup.h new file mode 100644 index 000000000..a8e221e02 --- /dev/null +++ b/examples/RobotSimulator/MinitaurSetup.h @@ -0,0 +1,20 @@ +#ifndef MINITAUR_SIMULATION_SETUP_H +#define MINITAUR_SIMULATION_SETUP_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Quaternion.h" +class MinitaurSetup +{ + struct MinitaurSetupInternalData* m_data; + +public: + MinitaurSetup(); + virtual ~MinitaurSetup(); + + int setupMinitaur(class b3RobotSimulatorClientAPI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1)); + + void resetPose(); + +}; +#endif //MINITAUR_SIMULATION_SETUP_H + \ No newline at end of file diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp new file mode 100644 index 000000000..dbea6bdf7 --- /dev/null +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -0,0 +1,66 @@ + +#include "b3RobotSimulatorClientAPI.h" +#include "../Utils/b3Clock.h" + +#include +#include +#include +#define ASSERT_EQ(a,b) assert((a)==(b)); +#include "MinitaurSetup.h" +int main(int argc, char* argv[]) +{ + b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI(); + + sim->connect(eCONNECT_GUI); + //Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example: + //sim->connect(eCONNECT_UDP, "localhost", 1234); + sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); + sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME + + //syncBodies is only needed when connecting to an existing physics server that has already some bodies + sim->syncBodies(); + + sim->setTimeStep(1./240.); + + sim->setGravity(b3MakeVector3(0,0,-10)); + + sim->loadURDF("plane.urdf"); + + MinitaurSetup minitaur; + int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,1)); + + + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition.setValue(2,0,1); + int r2d2 = sim->loadURDF("r2d2.urdf",args); + + b3RobotSimulatorLoadFileResults sdfResults; + if (!sim->loadSDF("two_cubes.sdf",sdfResults)) + { + b3Warning("Can't load SDF!\n"); + } + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double simWallClockSeconds = 20.; + + while (clock.getTimeInSeconds()-startTime < simWallClockSeconds) + { + sim->stepSimulation(); + } + + sim->setRealTimeSimulation(true); + + startTime = clock.getTimeInSeconds(); + while (clock.getTimeInSeconds()-startTime < simWallClockSeconds) + { + b3Clock::usleep(1000); + } + + sim->disconnect(); + delete sim; + + +} + + diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp new file mode 100644 index 000000000..b897abf29 --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -0,0 +1,776 @@ +#include "b3RobotSimulatorClientAPI.h" + + +//#include "SharedMemoryCommands.h" + +#include "SharedMemory/PhysicsClientC_API.h" + +#ifdef BT_ENABLE_ENET +#include "SharedMemory/PhysicsClientUDP_C_API.h" +#endif//PHYSICS_UDP + +#ifdef BT_ENABLE_CLSOCKET +#include "SharedMemory/PhysicsClientTCP_C_API.h" +#endif//PHYSICS_TCP + +#include "SharedMemory/PhysicsDirectC_API.h" + +#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h" + +#include "SharedMemory/SharedMemoryPublic.h" +#include "Bullet3Common/b3Logging.h" + +struct b3RobotSimulatorClientAPI_InternalData +{ + b3PhysicsClientHandle m_physicsClientHandle; + + b3RobotSimulatorClientAPI_InternalData() + :m_physicsClientHandle(0) + { + } +}; + +b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI() +{ + m_data = new b3RobotSimulatorClientAPI_InternalData(); +} + + b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() +{ + delete m_data; +} + + +bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) +{ + if (m_data->m_physicsClientHandle) + { + b3Warning ("Already connected, disconnect first."); + return false; + } + b3PhysicsClientHandle sm = 0; + + int udpPort = 1234; + int tcpPort = 6667; + int key = SHARED_MEMORY_KEY; + bool connected = false; + + + switch (mode) + { + case eCONNECT_GUI: + { + int argc = 0; + char* argv[1] = {0}; + #ifdef __APPLE__ + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + #else + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + #endif + break; + } + case eCONNECT_DIRECT: { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: { + if (portOrKey>=0) + { + key = portOrKey; + } + sm = b3ConnectSharedMemory(key); + break; + } + case eCONNECT_UDP: + { + if (portOrKey>=0) + { + udpPort = portOrKey; + } + #ifdef BT_ENABLE_ENET + + sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort); + #else + b3Warning("UDP is not enabled in this build"); + #endif //BT_ENABLE_ENET + + break; + } + case eCONNECT_TCP: + { + if (portOrKey>=0) + { + tcpPort = portOrKey; + } + #ifdef BT_ENABLE_CLSOCKET + + sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort); + #else + b3Warning("TCP is not enabled in this pybullet build"); + #endif //BT_ENABLE_CLSOCKET + break; + } + + + + default: { + b3Warning("connectPhysicsServer unexpected argument"); + } + }; + + if (sm) + { + m_data->m_physicsClientHandle = sm; + if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + disconnect(); + return false; + } + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI::isConnected() const +{ + return (m_data->m_physicsClientHandle!=0); +} + + +void b3RobotSimulatorClientAPI::disconnect() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3DisconnectSharedMemory(m_data->m_physicsClientHandle); + m_data->m_physicsClientHandle = 0; +} + +void b3RobotSimulatorClientAPI::syncBodies() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + +} + +void b3RobotSimulatorClientAPI::resetSimulation() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); +} + +void b3RobotSimulatorClientAPI::stepSimulation() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); + } +} + +void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); +} + +b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) +{ + double phi, the, psi; + phi = rollPitchYaw[0] / 2.0; + the = rollPitchYaw[1] / 2.0; + psi = rollPitchYaw[2] / 2.0; + double quat[4] = { + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; + + // normalize the quaternion + double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + + quat[2] * quat[2] + quat[3] * quat[3]); + quat[0] /= len; + quat[1] /= len; + quat[2] /= len; + quat[3] /= len; + b3Quaternion q(quat[0],quat[1],quat[2],quat[3]); + return q; +} + +b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) +{ + double squ; + double sqx; + double sqy; + double sqz; + double sarg; + + b3Vector3 rpy; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), + squ - sqx - sqy + sqz); + sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 + : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), + squ + sqx - sqy - sqz); + return rpy; +} + +int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) +{ + int robotUniqueId = -1; + + if (!isConnected()) + { + b3Warning("Not connected"); + return robotUniqueId; + } + + + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + + //setting the initial position, orientation and other arguments are optional + + b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], + args.m_startPosition[1], + args.m_startPosition[2]); + b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] + ,args.m_startOrientation[1] + ,args.m_startOrientation[2] + ,args.m_startOrientation[3]); + if (args.m_forceOverrideFixedBase) + { + b3LoadUrdfCommandSetUseFixedBase(command,true); + } + b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + + b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); + if (statusType==CMD_URDF_LOADING_COMPLETED) + { + robotUniqueId = b3GetStatusBodyIndex(statusHandle); + } + return robotUniqueId; +} + +bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_MJCF_LOADING_COMPLETED) + { + b3Warning("Couldn't load .mjcf file."); + return false; + } + int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + if (numBodies) + { + results.m_uniqueObjectIds.resize(numBodies); + int numBodies; + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + } + + return true; +} + +bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +{ + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_BULLET_LOADING_COMPLETED) + { + return false; + } + int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + if (numBodies) + { + results.m_uniqueObjectIds.resize(numBodies); + int numBodies; + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + } + + return true; +} + +bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + bool statusOk = false; + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); + if (statusType == CMD_SDF_LOADING_COMPLETED) + { + int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + if (numBodies) + { + results.m_uniqueObjectIds.resize(numBodies); + int numBodies; + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + + } + statusOk = true; + } + + return statusOk; +} + + +bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const +{ + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); + + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + return false; + } + + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, &actualStateQ, + 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + + basePosition[0] = actualStateQ[0]; + basePosition[1] = actualStateQ[1]; + basePosition[2] = actualStateQ[2]; + + baseOrientation[0] = actualStateQ[3]; + baseOrientation[1] = actualStateQ[4]; + baseOrientation[2] = actualStateQ[5]; + baseOrientation[3] = actualStateQ[6]; + return true; +} + +bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) +{ + b3SharedMemoryCommandHandle commandHandle; + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); + b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], + baseOrientation[2], baseOrientation[3]); + + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + + return true; +} + + +void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + + int ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + +int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId); +} + + +bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0); +} + +void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); + } +} + +bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + int statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) + { + return true; + } + } + return false; +} + +bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, int targetValue) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + return false; + } + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + return false; +} + + +void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + switch (args.m_controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + b3JointControlSetKd(command,uIndex,args.m_kd); + b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); + b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + int qIndex = jointInfo.m_qIndex; + + b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); + b3JointControlSetKp(command,uIndex,args.m_kp); + b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); + b3JointControlSetKd(command,uIndex,args.m_kd); + b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + break; + } + case CONTROL_MODE_TORQUE: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + break; + } + default: + { + b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl"); + } + } +} + + +void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSolverIterations(command, numIterations); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); + +} + +void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + int ret; + ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + +} + + +void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); +} + + +bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3Assert(args.m_endEffectorLinkIndex>=0); + b3Assert(args.m_bodyUniqueId>=0); + + + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId); + if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) + { + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); + } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); + } + + if (args.m_flags & B3_HAS_JOINT_DAMPING) + { + b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); + } + + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + + + int numPos=0; + + bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &numPos, + 0)!=0; + if (result && numPos) + { + results.m_calculatedJointPositions.resize(numPos); + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &numPos, + &results.m_calculatedJointPositions[0])!=0; + + } + return result; +} + + + +bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) + { + b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); + return true; + } + return false; +} + +void b3RobotSimulatorClientAPI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); + b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + +int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds) +{ + int loggingUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); + + b3StateLoggingStart(commandHandle,loggingType,fileName.c_str()); + + for ( int i=0;im_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_STATE_LOGGING_START_COMPLETED) + { + loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); + } + return loggingUniqueId; +} + +void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId) +{ + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); + b3StateLoggingStop(commandHandle, stateLoggerUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h new file mode 100644 index 000000000..09951ed32 --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -0,0 +1,191 @@ +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_H + +#include "../SharedMemory/SharedMemoryPublic.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include + + + + + +struct b3RobotSimulatorLoadUrdfFileArgs +{ + b3Vector3 m_startPosition; + b3Quaternion m_startOrientation; + bool m_forceOverrideFixedBase; + bool m_useMultiBody; + + b3RobotSimulatorLoadUrdfFileArgs() + : + m_startPosition(b3MakeVector3(0,0,0)), + m_startOrientation(b3Quaternion(0,0,0,1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true) + { + } +}; + +struct b3RobotSimulatorLoadSdfFileArgs +{ + bool m_forceOverrideFixedBase; + bool m_useMultiBody; + + b3RobotSimulatorLoadSdfFileArgs() + : + m_forceOverrideFixedBase(false), + m_useMultiBody(true) + { + } +}; + + +struct b3RobotSimulatorLoadFileResults +{ + b3AlignedObjectArray m_uniqueObjectIds; + b3RobotSimulatorLoadFileResults() + { + } +}; + +struct b3RobotSimulatorJointMotorArgs +{ + int m_controlMode; + + double m_targetPosition; + double m_kp; + + double m_targetVelocity; + double m_kd; + + double m_maxTorqueValue; + + b3RobotSimulatorJointMotorArgs(int controlMode) + :m_controlMode(controlMode), + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.9), + m_maxTorqueValue(1000) + { + } +}; + +enum b3RobotSimulatorInverseKinematicsFlags +{ + B3_HAS_IK_TARGET_ORIENTATION=1, + B3_HAS_NULL_SPACE_VELOCITY=2, + B3_HAS_JOINT_DAMPING=4, +}; + +struct b3RobotSimulatorInverseKinematicArgs +{ + int m_bodyUniqueId; +// double* m_currentJointPositions; +// int m_numPositions; + double m_endEffectorTargetPosition[3]; + double m_endEffectorTargetOrientation[4]; + int m_endEffectorLinkIndex; + int m_flags; + int m_numDegreeOfFreedom; + b3AlignedObjectArray m_lowerLimits; + b3AlignedObjectArray m_upperLimits; + b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; + b3AlignedObjectArray m_jointDamping; + + b3RobotSimulatorInverseKinematicArgs() + :m_bodyUniqueId(-1), + m_endEffectorLinkIndex(-1), + m_flags(0) + { + m_endEffectorTargetPosition[0]=0; + m_endEffectorTargetPosition[1]=0; + m_endEffectorTargetPosition[2]=0; + + m_endEffectorTargetOrientation[0]=0; + m_endEffectorTargetOrientation[1]=0; + m_endEffectorTargetOrientation[2]=0; + m_endEffectorTargetOrientation[3]=1; + } +}; + +struct b3RobotSimulatorInverseKinematicsResults +{ + int m_bodyUniqueId; + b3AlignedObjectArray m_calculatedJointPositions; +}; + + +///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet +///as documented in the pybullet Quickstart Guide +///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA +class b3RobotSimulatorClientAPI +{ + struct b3RobotSimulatorClientAPI_InternalData* m_data; + +public: + b3RobotSimulatorClientAPI(); + virtual ~b3RobotSimulatorClientAPI(); + + bool connect(int mode, const std::string& hostName="localhost", int portOrKey=-1); + + void disconnect(); + + bool isConnected() const; + + void syncBodies(); + + void resetSimulation(); + + b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); + b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); + + int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); + bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); + bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); + + bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; + bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); + + int getNumJoints(int bodyUniqueId) const; + + bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); + + void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + + bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); + + bool resetJointState(int bodyUniqueId, int jointIndex, int targetValue); + + void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); + + void stepSimulation(); + + void setRealTimeSimulation(bool enableRealTimeSimulation); + + void setGravity(const b3Vector3& gravityAcceleration); + + void setTimeStep(double timeStepInSeconds); + void setNumSimulationSubSteps(int numSubSteps); + void setNumSolverIterations(int numIterations); + + bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); + + bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); + + bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); + + void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); + + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds); + void stopStateLogging(int stateLoggerUniqueId); + +}; + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua new file mode 100644 index 000000000..7eb0e1b55 --- /dev/null +++ b/examples/RobotSimulator/premake4.lua @@ -0,0 +1,158 @@ +project ("App_RobotSimulator") + + language "C++" + kind "ConsoleApp" + + includedirs {"../../src", "../../examples", + "../../examples/ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + + hasCL = findOpenCL("clew") + + links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"} + initOpenGL() + initGlew() + + includedirs { + ".", + "../../src", + "../ThirdPartyLibs", + } + + + if os.is("MacOSX") then + links{"Cocoa.framework"} + end + + if (hasCL) then + links { + "Bullet3OpenCL_clew", + "Bullet3Dynamics", + "Bullet3Collision", + "Bullet3Geometry", + "Bullet3Common", + } + end + + + +if not _OPTIONS["no-enet"] then + + includedirs {"../../examples/ThirdPartyLibs/enet/include"} + + if os.is("Windows") then +-- targetextension {"dylib"} + defines { "WIN32" } + links {"Ws2_32","Winmm"} + end + if os.is("Linux") then + end + if os.is("MacOSX") then + end + + links {"enet"} + + files { + "../../examples/SharedMemory/PhysicsClientUDP.cpp", + "../../examples/SharedMemory/PhysicsClientUDP.h", + "../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientUDP_C_API.h", + } + defines {"BT_ENABLE_ENET"} + end + + if not _OPTIONS["no-clsocket"] then + + includedirs {"../../examples/ThirdPartyLibs/clsocket/src"} + + if os.is("Windows") then + defines { "WIN32" } + links {"Ws2_32","Winmm"} + end + if os.is("Linux") then + defines {"_LINUX"} + end + if os.is("MacOSX") then + defines {"_DARWIN"} + end + + links {"clsocket"} + + files { + "../../examples/SharedMemory/PhysicsClientTCP.cpp", + "../../examples/SharedMemory/PhysicsClientTCP.h", + "../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientTCP_C_API.h", + } + defines {"BT_ENABLE_CLSOCKET"} + end + + files { + "RobotSimulatorMain.cpp", + "b3RobotSimulatorClientAPI.cpp", + "b3RobotSimulatorClientAPI.h", + "MinitaurSetup.cpp", + "MinitaurSetup.h", + "../../examples/SharedMemory/IKTrajectoryHelper.cpp", + "../../examples/SharedMemory/IKTrajectoryHelper.h", + "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", + "../../examples/SharedMemory/InProcessMemory.cpp", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsServerExample.cpp", + "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsDirect.cpp", + "../../examples/SharedMemory/PhysicsDirect.h", + "../../examples/SharedMemory/PhysicsDirectC_API.cpp", + "../../examples/SharedMemory/PhysicsDirectC_API.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp", + "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h", + "../../examples/TinyRenderer/geometry.cpp", + "../../examples/TinyRenderer/model.cpp", + "../../examples/TinyRenderer/tgaimage.cpp", + "../../examples/TinyRenderer/our_gl.cpp", + "../../examples/TinyRenderer/TinyRenderer.cpp", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/Utils/RobotLoggingUtil.cpp", + "../../examples/Utils/RobotLoggingUtil.h", + "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/MultiThreading/b3PosixThreadSupport.cpp", + "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + } + if os.is("Linux") then + initX11() + end diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index a2c852581..b282c09cb 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -94,7 +94,7 @@ LINK_LIBRARIES( ) IF (WIN32) - ADD_EXECUTABLE(App_SharedMemoryPhysics + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory ${SharedMemory_SRCS} main.cpp ../MultiThreading/b3Win32ThreadSupport.cpp @@ -104,7 +104,7 @@ IF (WIN32) ELSE(WIN32) IF(APPLE) LINK_LIBRARIES( pthread dl ) - ADD_EXECUTABLE(App_SharedMemoryPhysics + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory ${SharedMemory_SRCS} ../MultiThreading/b3PosixThreadSupport.cpp ../MultiThreading/b3PosixThreadSupport.h @@ -113,7 +113,7 @@ ELSE(WIN32) ELSE(APPLE) LINK_LIBRARIES( pthread dl ) - ADD_EXECUTABLE(App_SharedMemoryPhysics + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory ${SharedMemory_SRCS} ../MultiThreading/b3PosixThreadSupport.cpp ../MultiThreading/b3PosixThreadSupport.h @@ -126,9 +126,9 @@ ENDIF(WIN32) IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics PROPERTIES DEBUG_POSTFIX "_Debug") - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) @@ -151,7 +151,7 @@ IF (WIN32) ADD_DEFINITIONS(-DGLEW_STATIC) LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) - ADD_EXECUTABLE(App_SharedMemoryPhysics_GUI + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_GUI ${SharedMemory_SRCS} ../StandaloneMain/main_opengl_single_example.cpp ../ExampleBrowser/OpenGLGuiHelper.cpp @@ -168,7 +168,7 @@ ELSE(WIN32) MESSAGE(${COCOA}) LINK_LIBRARIES(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) - ADD_EXECUTABLE(App_SharedMemoryPhysics_GUI + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_GUI ${SharedMemory_SRCS} ../StandaloneMain/main_opengl_single_example.cpp ../ExampleBrowser/OpenGLGuiHelper.cpp @@ -184,7 +184,7 @@ ELSE(WIN32) ADD_DEFINITIONS("-DGLEW_STATIC") ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") - ADD_EXECUTABLE(App_SharedMemoryPhysics_GUI + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_GUI ${SharedMemory_SRCS} ../StandaloneMain/main_opengl_single_example.cpp ../ExampleBrowser/OpenGLGuiHelper.cpp @@ -200,9 +200,9 @@ ENDIF(WIN32) IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_GUI PROPERTIES DEBUG_POSTFIX "_Debug") - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_GUI PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_GUI PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_GUI PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_GUI PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_GUI PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) @@ -232,7 +232,7 @@ LINK_LIBRARIES( ELSE(CMAKE_CL_64) LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/lib/win32) ENDIF(CMAKE_CL_64) - ADD_EXECUTABLE(App_SharedMemoryPhysics_VR + ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_VR ${SharedMemory_SRCS} ../StandaloneMain/hellovr_opengl_main.cpp ../ExampleBrowser/OpenGLGuiHelper.cpp @@ -257,35 +257,35 @@ LINK_LIBRARIES( IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) IF (CMAKE_CL_64) ADD_CUSTOM_COMMAND( - TARGET App_SharedMemoryPhysics_VR + TARGET App_PhysicsServer_SharedMemory_VR POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/win64/openvr_api.dll ${CMAKE_CURRENT_BINARY_DIR}/openvr_api.dll ) ELSE(CMAKE_CL_64) ADD_CUSTOM_COMMAND( - TARGET App_SharedMemoryPhysics_VR + TARGET App_PhysicsServer_SharedMemory_VR POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/win32/openvr_api.dll ${CMAKE_CURRENT_BINARY_DIR}/openvr_api.dll ) ENDIF(CMAKE_CL_64) ADD_CUSTOM_COMMAND( - TARGET App_SharedMemoryPhysics_VR + TARGET App_PhysicsServer_SharedMemory_VR POST_BUILD COMMAND ${CMAKE_COMMAND} ARGS -E copy_directory ${BULLET_PHYSICS_SOURCE_DIR}/data ${PROJECT_BINARY_DIR}/data ) ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) -SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_VR PROPERTIES COMPILE_DEFINITIONS BT_ENABLE_VR ) +SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES COMPILE_DEFINITIONS BT_ENABLE_VR ) IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_VR PROPERTIES DEBUG_POSTFIX "_Debug") - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_VR PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") - SET_TARGET_PROPERTIES(App_SharedMemoryPhysics_VR PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) ENDIF(WIN32) \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 1fdb07ad4..b2c7b2faf 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -60,6 +60,9 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0; + virtual void setTimeOut(double timeOutInSeconds) = 0; + virtual double getTimeOut() const = 0; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 58f3c7c97..3013fe31d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1040,13 +1040,17 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan { b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + b3SharedMemoryStatusHandle statusHandle = 0; b3Assert(commandHandle); b3Assert(physClient); if (physClient && commandHandle) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + + double timeOutInSeconds = cl->getTimeOut(); + b3SubmitClientCommand(physClient, commandHandle); while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) @@ -2604,3 +2608,24 @@ void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle comman } } +void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + if (cl) + { + cl->setTimeOut(timeOutInSeconds); + } + +} + +double b3GetTimeOut(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + if (cl) + { + return cl->getTimeOut(); + } + return -1; +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 61638bb7f..5812a4423 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -355,6 +355,10 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); +void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); +double b3GetTimeOut(b3PhysicsClientHandle physClient); + + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c1b82b740..6cbf587d6 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -60,6 +60,7 @@ struct PhysicsClientSharedMemoryInternalData { bool m_hasLastServerStatus; int m_sharedMemoryKey; bool m_verboseOutput; + double m_timeOutInSeconds; PhysicsClientSharedMemoryInternalData() : m_sharedMemory(0), @@ -72,7 +73,9 @@ struct PhysicsClientSharedMemoryInternalData { m_waitingForServer(false), m_hasLastServerStatus(false), m_sharedMemoryKey(SHARED_MEMORY_KEY), - m_verboseOutput(false) {} + m_verboseOutput(false), + m_timeOutInSeconds(1e30) + {} void processServerStatus(); @@ -1208,3 +1211,11 @@ const float* PhysicsClientSharedMemory::getDebugLinesColor() const { } int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); } +void PhysicsClientSharedMemory::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} +double PhysicsClientSharedMemory::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 57392d2b2..1a56dad14 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -69,6 +69,9 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientTCP.cpp b/examples/SharedMemory/PhysicsClientTCP.cpp index 4961dad01..4215769e1 100644 --- a/examples/SharedMemory/PhysicsClientTCP.cpp +++ b/examples/SharedMemory/PhysicsClientTCP.cpp @@ -47,11 +47,13 @@ struct TcpNetworkedInternalData int m_port; b3AlignedObjectArray m_tempBuffer; + double m_timeOutInSeconds; TcpNetworkedInternalData() : m_isConnected(false), - m_hasCommand(false) + m_hasCommand(false), + m_timeOutInSeconds(60) { } @@ -66,8 +68,8 @@ struct TcpNetworkedInternalData m_isConnected = m_tcpSocket.Open(m_hostName.c_str(),m_port); if (m_isConnected) { - m_tcpSocket.SetSendTimeout(5,0); - m_tcpSocket.SetReceiveTimeout(5,0); + m_tcpSocket.SetSendTimeout(m_timeOutInSeconds,0); + m_tcpSocket.SetReceiveTimeout(m_timeOutInSeconds,0); } int key = SHARED_MEMORY_MAGIC_NUMBER; m_tcpSocket.Send((uint8*)&key,4); @@ -257,7 +259,10 @@ void TcpNetworkedPhysicsProcessor::disconnect() } - +void TcpNetworkedPhysicsProcessor::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} diff --git a/examples/SharedMemory/PhysicsClientTCP.h b/examples/SharedMemory/PhysicsClientTCP.h index 26dd45b58..12644eb68 100644 --- a/examples/SharedMemory/PhysicsClientTCP.h +++ b/examples/SharedMemory/PhysicsClientTCP.h @@ -30,6 +30,8 @@ public: virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); + virtual void setTimeOut(double timeOutInSeconds); + }; diff --git a/examples/SharedMemory/PhysicsClientUDP.cpp b/examples/SharedMemory/PhysicsClientUDP.cpp index 4da08895a..d6ec32d5e 100644 --- a/examples/SharedMemory/PhysicsClientUDP.cpp +++ b/examples/SharedMemory/PhysicsClientUDP.cpp @@ -80,6 +80,7 @@ struct UdpNetworkedInternalData std::string m_hostName; int m_port; + double m_timeOutInSeconds; UdpNetworkedInternalData() :m_client(0), @@ -87,7 +88,8 @@ struct UdpNetworkedInternalData m_isConnected(false), m_threadSupport(0), m_hasCommand(false), - m_hasStatus(false) + m_hasStatus(false), + m_timeOutInSeconds(60) { } @@ -467,7 +469,7 @@ bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; m_data->m_cs->lock(); m_data->m_clientCmd = clientCmd; @@ -486,7 +488,7 @@ bool UdpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; const SharedMemoryStatus* stat = 0; while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) @@ -621,7 +623,7 @@ void UdpNetworkedPhysicsProcessor::disconnect() } - - - - +void UdpNetworkedPhysicsProcessor::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} diff --git a/examples/SharedMemory/PhysicsClientUDP.h b/examples/SharedMemory/PhysicsClientUDP.h index 27b94fd48..bcb4e3268 100644 --- a/examples/SharedMemory/PhysicsClientUDP.h +++ b/examples/SharedMemory/PhysicsClientUDP.h @@ -30,6 +30,7 @@ public: virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); + virtual void setTimeOut(double timeOutInSeconds); }; diff --git a/examples/SharedMemory/PhysicsCommandProcessorInterface.h b/examples/SharedMemory/PhysicsCommandProcessorInterface.h index 39b5f36d5..e883b86f4 100644 --- a/examples/SharedMemory/PhysicsCommandProcessorInterface.h +++ b/examples/SharedMemory/PhysicsCommandProcessorInterface.h @@ -20,7 +20,7 @@ public: virtual void renderScene() = 0; virtual void physicsDebugDraw(int debugDrawFlags) = 0; virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) = 0; - + virtual void setTimeOut(double timeOutInSeconds) = 0; }; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 892f6682b..fd8b9f28b 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -56,11 +56,13 @@ struct PhysicsDirectInternalData PhysicsCommandProcessorInterface* m_commandProcessor; bool m_ownsCommandProcessor; + double m_timeOutInSeconds; PhysicsDirectInternalData() :m_hasStatus(false), m_verboseOutput(false), - m_ownsCommandProcessor(false) + m_ownsCommandProcessor(false), + m_timeOutInSeconds(1e30) { } }; @@ -230,7 +232,7 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -315,7 +317,7 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -369,7 +371,7 @@ bool PhysicsDirect::processOverlappingObjects(const struct SharedMemoryCommand& b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -427,7 +429,7 @@ bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& or b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -491,7 +493,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -754,7 +756,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -781,7 +783,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Clock clock; double startTime = clock.getTimeInSeconds(); - double timeOutInSeconds = 10; + double timeOutInSeconds = m_data->m_timeOutInSeconds; while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { @@ -1034,3 +1036,13 @@ void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHit raycastHits->m_numRayHits = m_data->m_raycastHits.size(); raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; } + +void PhysicsDirect::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} + +double PhysicsDirect::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index a8b4c6aac..0e78cf794 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -95,6 +95,8 @@ public: virtual void renderScene(); virtual void debugDraw(int debugDrawMode); + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; }; #endif //PHYSICS_DIRECT_H diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 6de63e374..6ed6ac8a2 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -169,3 +169,13 @@ void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastH { return m_data->m_physicsClient->getCachedRaycastHits(raycastHits); } + +void PhysicsLoopBack::setTimeOut(double timeOutInSeconds) +{ + m_data->m_physicsClient->setTimeOut(timeOutInSeconds); +} +double PhysicsLoopBack::getTimeOut() const +{ + return m_data->m_physicsClient->getTimeOut(); +} + diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 3cbeba296..17758510e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -74,6 +74,8 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; }; #endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9521b2c16..f4de70350 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5713,3 +5713,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() } } + +void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/) +{ +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index f90716665..ff3a706a6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -99,6 +99,8 @@ public: void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents); void enableRealTimeSimulation(bool enableRealTimeSim); void applyJointDamping(int bodyUniqueId); + + virtual void setTimeOut(double timeOutInSeconds); }; #endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/SharedMemoryCommandProcessor.cpp b/examples/SharedMemory/SharedMemoryCommandProcessor.cpp index f488d27d6..90980794e 100644 --- a/examples/SharedMemory/SharedMemoryCommandProcessor.cpp +++ b/examples/SharedMemory/SharedMemoryCommandProcessor.cpp @@ -212,4 +212,6 @@ void SharedMemoryCommandProcessor::setSharedMemoryKey(int key) m_data->m_sharedMemoryKey = key; } - +void SharedMemoryCommandProcessor::setTimeOut(double /*timeOutInSeconds*/) +{ +} diff --git a/examples/SharedMemory/SharedMemoryCommandProcessor.h b/examples/SharedMemory/SharedMemoryCommandProcessor.h index bec9e614f..b552b7ad3 100644 --- a/examples/SharedMemory/SharedMemoryCommandProcessor.h +++ b/examples/SharedMemory/SharedMemoryCommandProcessor.h @@ -29,7 +29,7 @@ public: void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); void setSharedMemoryKey(int key); - + virtual void setTimeOut(double timeOutInSeconds); }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 131c4049a..a123bbf49 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -310,7 +310,7 @@ enum CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1, }; -enum +enum b3StateLoggingType { STATE_LOGGING_MINITAUR = 0, STATE_LOGGING_GENERIC_ROBOT = 1, @@ -403,10 +403,19 @@ enum EnumRenderer //ER_FIRE_RAYS=(1<<18), }; -enum EnumConfigureOpenGLVisualizer +enum b3ConfigureDebugVisualizerEnum { COV_ENABLE_GUI=1, COV_ENABLE_SHADOWS, COV_ENABLE_WIREFRAME, }; + +enum eCONNECT_METHOD { + eCONNECT_GUI = 1, + eCONNECT_DIRECT = 2, + eCONNECT_SHARED_MEMORY = 3, + eCONNECT_UDP = 4, + eCONNECT_TCP = 5, +}; + #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index e14a0146b..e410dbf9c 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -1,5 +1,5 @@ -project "App_SharedMemoryPhysics" +project "App_PhysicsServer_SharedMemory" if _OPTIONS["ios"] then kind "WindowedApp" @@ -141,7 +141,7 @@ files { end -project "App_SharedMemoryPhysics_GUI" +project "App_PhysicsServer_SharedMemory_GUI" if _OPTIONS["ios"] then kind "WindowedApp" @@ -240,8 +240,10 @@ if os.is("MacOSX") then --defines {"__MACOSX_CORE__"} end + + if os.is("Windows") then - project "App_SharedMemoryPhysics_VR" + project "App_PhysicsServer_SharedMemory_VR" --for now, only enable VR under Windows, until compilation issues are resolved on Mac/Linux defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"} diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 41e0a6c07..eadc48fb7 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -153,7 +153,7 @@ SET_TARGET_PROPERTIES(pybullet PROPERTIES DEBUG_POSTFIX "_d") IF(WIN32) IF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET) TARGET_LINK_LIBRARIES(pybullet ws2_32 ) - ENDIF(BUILD_PYBULLET_ENET) + ENDIF(BUILD_PYBULLET_ENET OR BUILD_PYBULLET_CLSOCKET) SET_TARGET_PROPERTIES(pybullet PROPERTIES SUFFIX ".pyd" ) ENDIF(WIN32) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 206273f58..39806692f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -24,13 +24,7 @@ #define PyString_FromString PyBytes_FromString #endif -enum eCONNECT_METHOD { - eCONNECT_GUI = 1, - eCONNECT_DIRECT = 2, - eCONNECT_SHARED_MEMORY = 3, - eCONNECT_UDP = 4, - eCONNECT_TCP = 5, -}; + static PyObject* SpamError; @@ -2680,7 +2674,6 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds) { - b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; int loggingId=-1; @@ -2712,7 +2705,30 @@ static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObj return Py_None; } +static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "timeOutInSeconds", "physicsClientId", NULL }; + double timeOutInSeconds = -1; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, + &timeOutInSeconds, &physicsClientId)) + return NULL; + if (timeOutInSeconds>=0) + { + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + b3SetTimeOut(sm,timeOutInSeconds); + } + + Py_INCREF(Py_None); + return Py_None; +} static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) { @@ -5136,8 +5152,9 @@ static PyMethodDef SpamMethods[] = { "Cast a ray and return the first object hit, if any. " "Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates" }, - - + { "setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, + "Set the timeOut in seconds, used for most of the API calls." + }, // todo(erwincoumans) // saveSnapshot // loadSnapshot