From 7bd84740d7277d00fa4ef298ad9d0d8a2229e19c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 5 Jun 2018 11:41:41 +1000 Subject: [PATCH] PyBullet / BulletRobotics: prepare for pdControlPlugin and collisionFilterPlugin Split examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.* and move to examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp and examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp --- Extras/BulletRobotics/CMakeLists.txt | 9 +- examples/ExampleBrowser/CMakeLists.txt | 6 +- examples/ExampleBrowser/premake4.lua | 6 +- .../b3RobotSimulatorClientAPI.cpp | 2 +- .../b3RobotSimulatorClientAPI.h | 2 +- examples/RobotSimulator/premake4.lua | 8 +- .../PhysicsServerCommandProcessor.cpp | 24 +- .../b3RobotSimulatorClientAPI_InternalData.h | 0 .../b3RobotSimulatorClientAPI_NoDirect.cpp} | 216 ++++++++---------- .../b3RobotSimulatorClientAPI_NoDirect.h} | 24 +- .../b3RobotSimulatorClientAPI_NoGUI.cpp | 112 +++++++++ .../b3RobotSimulatorClientAPI_NoGUI.h | 22 ++ .../collisionFilterPlugin.cpp | 98 ++++++++ .../collisionFilterPlugin.h | 25 ++ .../collisionFilterPlugin/premake4.lua | 42 ++++ .../pdControlPlugin/pdControlPlugin.cpp | 162 +++++++++++++ .../plugins/pdControlPlugin/pdControlPlugin.h | 33 +++ .../plugins/pdControlPlugin/premake4.lua | 44 ++++ .../plugins/testPlugin/testplugin.cpp | 56 +---- .../plugins/testPlugin/testplugin.h | 11 +- examples/SharedMemory/premake4.lua | 3 + examples/pybullet/examples/renderPlugin.py | 2 +- examples/pybullet/examples/testPlugin.py | 10 + 23 files changed, 719 insertions(+), 198 deletions(-) rename examples/{RobotSimulator => SharedMemory}/b3RobotSimulatorClientAPI_InternalData.h (100%) rename examples/{RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp => SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp} (84%) rename examples/{RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h => SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h} (97%) create mode 100644 examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp create mode 100644 examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h create mode 100644 examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp create mode 100644 examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h create mode 100644 examples/SharedMemory/plugins/collisionFilterPlugin/premake4.lua create mode 100644 examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp create mode 100644 examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h create mode 100644 examples/SharedMemory/plugins/pdControlPlugin/premake4.lua create mode 100644 examples/pybullet/examples/testPlugin.py diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 9ccfa7777..3ce9ee90a 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -9,6 +9,10 @@ INCLUDE_DIRECTORIES( ) SET(BulletRobotics_SRCS + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h ../../examples/SharedMemory/IKTrajectoryHelper.cpp ../../examples/SharedMemory/IKTrajectoryHelper.h ../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp @@ -91,8 +95,6 @@ SET(BulletRobotics_SRCS ../../examples/MultiThreading/b3PosixThreadSupport.cpp ../../examples/MultiThreading/b3Win32ThreadSupport.cpp ../../examples/MultiThreading/b3ThreadSupportInterface.cpp - ../../examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp - ../../examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h ) IF(BUILD_CLSOCKET) @@ -174,7 +176,8 @@ IF (INSTALL_EXTRA_LIBS) ../../examples/SharedMemory/PhysicsClientTCP_C_API.h ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h ../../examples/SharedMemory/SharedMemoryPublic.h - ../../examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h DESTINATION include/bullet ) INSTALL(TARGETS diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 149dcf576..d90c79822 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -175,10 +175,12 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/SharedMemoryCommands.h ../SharedMemory/SharedMemoryPublic.h ../SharedMemory/b3PluginManager.cpp + ../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp + ../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h + ../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp + ../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h ../RobotSimulator/b3RobotSimulatorClientAPI.cpp ../RobotSimulator/b3RobotSimulatorClientAPI.h - ../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp - ../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h ../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.h ../InverseDynamics/InverseDynamicsExample.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index be2df9704..2af26677d 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -119,6 +119,10 @@ project "App_BulletExampleBrowser" "../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", "../SharedMemory/SharedMemoryCommands.h", "../SharedMemory/SharedMemoryPublic.h", + "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", + "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h", + "../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", "../MultiThreading/MultiThreadingExample.cpp", "../MultiThreading/b3PosixThreadSupport.cpp", "../MultiThreading/b3Win32ThreadSupport.cpp", @@ -127,8 +131,6 @@ project "App_BulletExampleBrowser" "../InverseDynamics/InverseDynamicsExample.h", "../RobotSimulator/b3RobotSimulatorClientAPI.cpp", "../RobotSimulator/b3RobotSimulatorClientAPI.h", - "../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp", - "../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", "../ExtendedTutorials/*", diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 3268e351e..07ead0866 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -1,7 +1,7 @@ #include "b3RobotSimulatorClientAPI.h" #include "../SharedMemory/PhysicsClientC_API.h" -#include "b3RobotSimulatorClientAPI_InternalData.h" +#include "../SharedMemory/b3RobotSimulatorClientAPI_InternalData.h" #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //PHYSICS_UDP diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index a5f1a0cbd..ae2239462 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -1,7 +1,7 @@ #ifndef B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H #define B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H -#include "b3RobotSimulatorClientAPI_NoGUI.h" +#include "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h" ///The b3RobotSimulatorClientAPI_GUI is pretty much the C++ version of pybullet diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index 8c41b2707..cfa1e426b 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -1,6 +1,10 @@ myfiles = { + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp", @@ -191,8 +195,6 @@ if not _OPTIONS["no-enet"] then "RobotSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", - "b3RobotSimulatorClientAPI_NoGUI.cpp", - "b3RobotSimulatorClientAPI_NoGUI.h", "MinitaurSetup.cpp", "MinitaurSetup.h", myfiles @@ -283,8 +285,6 @@ project ("App_VRGloveHandSimulator") "VRGloveSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", - "b3RobotSimulatorClientAPI_NoGUI.cpp", - "b3RobotSimulatorClientAPI_NoGUI.h", myfiles } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0e1501b8a..c0f6885fc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -45,6 +45,9 @@ #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +#ifdef STATIC_PD_CONTROL_PLUGIN +#include "plugins/pdControlPlugin/pdControlPlugin.h" +#endif //STATIC_PD_CONTROL_PLUGIN #ifdef STATIC_LINK_VR_PLUGIN #include "plugins/vrSyncPlugin/vrSyncPlugin.h" @@ -1647,7 +1650,7 @@ struct PhysicsServerCommandProcessorInternalData btVector3 m_hitPos; btScalar m_oldPickingDist; bool m_prevCanSleep; - + int m_pdControlPlugin; #ifdef B3_ENABLE_TINY_AUDIO b3SoundEngine m_soundEngine; #endif @@ -1679,15 +1682,23 @@ struct PhysicsServerCommandProcessorInternalData m_verboseOutput(false), m_pickedBody(0), m_pickedConstraint(0), - m_pickingMultiBodyPoint2Point(0) + m_pickingMultiBodyPoint2Point(0), + m_pdControlPlugin(-1) { { //register static plugins: #ifdef STATIC_LINK_VR_PLUGIN - m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin,preTickPluginCallback_vrSyncPlugin,0,0); + m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0); #endif //STATIC_LINK_VR_PLUGIN +#ifdef STATIC_PD_CONTROL_PLUGIN + { + m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, postTickPluginCallback_pdControlPlugin, 0); + } +#endif //STATIC_PD_CONTROL_PLUGIN + + #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin,0,0,getRenderInterface_tinyRendererPlugin); m_pluginManager.selectPluginRenderer(renderPluginId); @@ -4949,6 +4960,13 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { +/* case CONTROL_MODE_PD: + { + b3PluginArguments args; + m_data->m_pluginManager.executePluginCommand(pdControlPlugin, args); + //#p.executePluginCommand(plugin ,"r2d2.urdf", [1,2,3],[50.0,3.3]) + } + */ case CONTROL_MODE_TORQUE: { if (m_data->m_verboseOutput) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_InternalData.h similarity index 100% rename from examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h rename to examples/SharedMemory/b3RobotSimulatorClientAPI_InternalData.h diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp similarity index 84% rename from examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp rename to examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index c025631a6..65ffcb9a4 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -1,20 +1,10 @@ -#include "b3RobotSimulatorClientAPI_NoGUI.h" +#include "b3RobotSimulatorClientAPI_NoDirect.h" -#include "../SharedMemory/PhysicsClientC_API.h" +#include "PhysicsClientC_API.h" #include "b3RobotSimulatorClientAPI_InternalData.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/SharedMemoryPublic.h" +#include "SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" static void scalarToDouble3(btScalar a[3], double b[3]) @@ -27,25 +17,25 @@ static void scalarToDouble3(btScalar a[3], double b[3]) static void scalarToDouble4(btScalar a[4], double b[4]) { - for (int i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) { b[i] = a[i]; } } -b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI() +b3RobotSimulatorClientAPI_NoDirect::b3RobotSimulatorClientAPI_NoDirect() { m_data = new b3RobotSimulatorClientAPI_InternalData(); } -b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI() +b3RobotSimulatorClientAPI_NoDirect::~b3RobotSimulatorClientAPI_NoDirect() { delete m_data; } -bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey) +bool b3RobotSimulatorClientAPI_NoDirect::connect(int mode, const std::string& hostName, int portOrKey) { if (m_data->m_physicsClientHandle) { @@ -53,7 +43,6 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN return false; } b3PhysicsClientHandle sm = 0; - int udpPort = 1234; int tcpPort = 6667; int key = SHARED_MEMORY_KEY; @@ -64,7 +53,6 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN case eCONNECT_DIRECT: { - sm = b3ConnectPhysicsDirect(); break; } case eCONNECT_SHARED_MEMORY: @@ -82,12 +70,6 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN { 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; } @@ -97,12 +79,6 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN { 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; } @@ -125,12 +101,12 @@ bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostN return false; } -bool b3RobotSimulatorClientAPI_NoGUI::isConnected() const +bool b3RobotSimulatorClientAPI_NoDirect::isConnected() const { return (m_data->m_physicsClientHandle != 0); } -void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec) +void b3RobotSimulatorClientAPI_NoDirect::setTimeOut(double timeOutInSec) { if (!isConnected()) { @@ -142,19 +118,22 @@ void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec) } -void b3RobotSimulatorClientAPI_NoGUI::disconnect() +void b3RobotSimulatorClientAPI_NoDirect::disconnect() { if (!isConnected()) { b3Warning("Not connected"); return; } - +#ifndef BT_DISABLE_PHYSICS_DIRECT b3DisconnectSharedMemory(m_data->m_physicsClientHandle); m_data->m_physicsClientHandle = 0; +#endif + } -void b3RobotSimulatorClientAPI_NoGUI::syncBodies() + +void b3RobotSimulatorClientAPI_NoDirect::syncBodies() { if (!isConnected()) { @@ -171,7 +150,7 @@ void b3RobotSimulatorClientAPI_NoGUI::syncBodies() statusType = b3GetStatusType(statusHandle); } -void b3RobotSimulatorClientAPI_NoGUI::resetSimulation() +void b3RobotSimulatorClientAPI_NoDirect::resetSimulation() { if (!isConnected()) { @@ -183,7 +162,7 @@ void b3RobotSimulatorClientAPI_NoGUI::resetSimulation() m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); } -bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const +bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const { if (!isConnected()) { @@ -192,7 +171,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); } -void b3RobotSimulatorClientAPI_NoGUI::stepSimulation() +void b3RobotSimulatorClientAPI_NoDirect::stepSimulation() { if (!isConnected()) { @@ -210,7 +189,7 @@ void b3RobotSimulatorClientAPI_NoGUI::stepSimulation() } } -void b3RobotSimulatorClientAPI_NoGUI::setGravity(const btVector3& gravityAcceleration) +void b3RobotSimulatorClientAPI_NoDirect::setGravity(const btVector3& gravityAcceleration) { if (!isConnected()) { @@ -226,14 +205,14 @@ void b3RobotSimulatorClientAPI_NoGUI::setGravity(const btVector3& gravityAcceler // btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } -btQuaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const btVector3& rollPitchYaw) +btQuaternion b3RobotSimulatorClientAPI_NoDirect::getQuaternionFromEuler(const btVector3& rollPitchYaw) { btQuaternion q; q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); return q; } -btVector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const btQuaternion& quat) +btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQuaternion& quat) { btScalar roll,pitch,yaw; quat.getEulerZYX(yaw,pitch,roll); @@ -241,7 +220,7 @@ btVector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const btQuater return rpy2; } -int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) +int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) { int robotUniqueId = -1; @@ -279,7 +258,7 @@ int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const return robotUniqueId; } -bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) { if (!isConnected()) { @@ -310,7 +289,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3Ro return true; } -bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +bool b3RobotSimulatorClientAPI_NoDirect::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) { if (!isConnected()) { @@ -340,7 +319,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3 return true; } -bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args) +bool b3RobotSimulatorClientAPI_NoDirect::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args) { if (!isConnected()) { @@ -372,7 +351,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3Rob return statusOk; } -bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) +bool b3RobotSimulatorClientAPI_NoDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) { if (!isConnected()) { @@ -384,7 +363,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3Bod return (result != 0); } -bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const +bool b3RobotSimulatorClientAPI_NoDirect::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const { if (!isConnected()) { @@ -422,7 +401,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniq return true; } -bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) +bool b3RobotSimulatorClientAPI_NoDirect::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) { if (!isConnected()) { @@ -444,7 +423,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUn return true; } -bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const +bool b3RobotSimulatorClientAPI_NoDirect::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const { if (!isConnected()) { @@ -478,7 +457,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, btVector return true; } -bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const +bool b3RobotSimulatorClientAPI_NoDirect::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const { if (!isConnected()) { @@ -503,7 +482,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const return true; } -void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags) +void b3RobotSimulatorClientAPI_NoDirect::setInternalSimFlags(int flags) { if (!isConnected()) { @@ -519,7 +498,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags) } -void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeSimulation) +void b3RobotSimulatorClientAPI_NoDirect::setRealTimeSimulation(bool enableRealTimeSimulation) { if (!isConnected()) { @@ -537,7 +516,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeS btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } -int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const +int b3RobotSimulatorClientAPI_NoDirect::getNumJoints(int bodyUniqueId) const { if (!isConnected()) { @@ -547,7 +526,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); } -bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) +bool b3RobotSimulatorClientAPI_NoDirect::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) { if (!isConnected()) { @@ -557,7 +536,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIn return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); } -int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) +int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) { if (!isConnected()) { @@ -579,7 +558,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int p return -1; } -int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointInfo* jointInfo) +int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3JointInfo* jointInfo) { if (!isConnected()) @@ -608,7 +587,7 @@ int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointI return statusType; } -void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId) +void b3RobotSimulatorClientAPI_NoDirect::removeConstraint(int constraintId) { if (!isConnected()) { @@ -621,7 +600,7 @@ void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId) } -bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) +bool b3RobotSimulatorClientAPI_NoDirect::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) { if (!isConnected()) { @@ -641,7 +620,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointI return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointStates2& state) +bool b3RobotSimulatorClientAPI_NoDirect::getJointStates(int bodyUniqueId, b3JointStates2& state) { if (!isConnected()) { @@ -700,7 +679,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointSt return false; } -bool b3RobotSimulatorClientAPI_NoGUI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue) +bool b3RobotSimulatorClientAPI_NoDirect::resetJointState(int bodyUniqueId, int jointIndex, double targetValue) { if (!isConnected()) { @@ -726,7 +705,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetJointState(int bodyUniqueId, int join return false; } -void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) +void b3RobotSimulatorClientAPI_NoDirect::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) { if (!isConnected()) { @@ -781,7 +760,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int } } -void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations) +void b3RobotSimulatorClientAPI_NoDirect::setNumSolverIterations(int numIterations) { if (!isConnected()) { @@ -795,7 +774,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations) btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } -void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double threshold) +void b3RobotSimulatorClientAPI_NoDirect::setContactBreakingThreshold(double threshold) { if (!isConnected()) { @@ -810,7 +789,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double thresho } -void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds) +void b3RobotSimulatorClientAPI_NoDirect::setTimeStep(double timeStepInSeconds) { if (!isConnected()) { @@ -825,7 +804,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds) statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); } -void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps) +void b3RobotSimulatorClientAPI_NoDirect::setNumSimulationSubSteps(int numSubSteps) { if (!isConnected()) { @@ -839,7 +818,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps) btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } -bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) +bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) { if (!isConnected()) { @@ -892,7 +871,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3 return result; } -bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) +bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) { if (!isConnected()) { @@ -912,7 +891,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int link } -bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) +bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) { bool computeLinkVelocity = true; bool computeForwardKinematics = true; @@ -920,7 +899,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkInd return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState); } -bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) +bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) { if (!isConnected()) { b3Warning("Not connected"); @@ -946,7 +925,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkInd return false; } -void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable) +void b3RobotSimulatorClientAPI_NoDirect::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable) { if (!isConnected()) { @@ -958,7 +937,7 @@ void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugV b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } -void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) +void b3RobotSimulatorClientAPI_NoDirect::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) { vrEventsData->m_numControllerEvents = 0; vrEventsData->m_controllerEvents = 0; @@ -974,7 +953,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,i b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); } -void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) +void b3RobotSimulatorClientAPI_NoDirect::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) { keyboardEventsData->m_numKeyboardEvents = 0; keyboardEventsData->m_keyboardEvents = 0; @@ -989,7 +968,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* ke b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); } -int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray& objectUniqueIds, int maxLogDof) +int b3RobotSimulatorClientAPI_NoDirect::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray& objectUniqueIds, int maxLogDof) { if (!isConnected()) { @@ -1024,7 +1003,7 @@ int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggin return loggingUniqueId; } -void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId) +void b3RobotSimulatorClientAPI_NoDirect::stopStateLogging(int stateLoggerUniqueId) { if (!isConnected()) { @@ -1039,7 +1018,7 @@ void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId) statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } -void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos) +void b3RobotSimulatorClientAPI_NoDirect::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos) { if (!isConnected()) { @@ -1060,7 +1039,7 @@ void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDi } } -void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds) +void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds) { if (!isConnected()) { @@ -1076,7 +1055,7 @@ void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string& pr b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } -void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin) +void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin) { if (!isConnected()) { @@ -1091,7 +1070,7 @@ void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); } -void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEventsData) +void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData) { mouseEventsData->m_numMouseEvents = 0; mouseEventsData->m_mouseEvents = 0; @@ -1106,7 +1085,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEve } -bool b3RobotSimulatorClientAPI_NoGUI::getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, struct b3CameraImageData &imageData) +bool b3RobotSimulatorClientAPI_NoDirect::getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, struct b3CameraImageData &imageData) { if (!isConnected()) { b3Warning("Not connected"); @@ -1172,7 +1151,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getCameraImage(int width, int height, stru return true; } -bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, +bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput) { if (!isConnected()) { @@ -1204,7 +1183,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, return false; } -int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const +int b3RobotSimulatorClientAPI_NoDirect::getNumBodies() const { if (!isConnected()) { b3Warning("Not connected"); @@ -1213,7 +1192,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const return b3GetNumBodies(m_data->m_physicsClientHandle); } -int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const +int b3RobotSimulatorClientAPI_NoDirect::getBodyUniqueId(int bodyId) const { if (!isConnected()) { b3Warning("Not connected"); @@ -1222,7 +1201,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId); } -bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId) +bool b3RobotSimulatorClientAPI_NoDirect::removeBody(int bodyUniqueId) { if (!isConnected()) { b3Warning("Not connected"); @@ -1246,7 +1225,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId) return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) { +bool b3RobotSimulatorClientAPI_NoDirect::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) { if (!isConnected()) { b3Warning("Not connected"); return false; @@ -1282,7 +1261,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int link } -bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args) +bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1331,7 +1310,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkI return true; } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) { +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { b3Warning("Not connected to physics server."); @@ -1354,7 +1333,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, dou } -double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) { +double b3RobotSimulatorClientAPI_NoDirect::readUserDebugParameter(int itemUniqueId) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { b3Warning("Not connected to physics server."); @@ -1380,7 +1359,7 @@ double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) } -bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) { +bool b3RobotSimulatorClientAPI_NoDirect::removeUserDebugItem(int itemUniqueId) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { b3Warning("Not connected to physics server."); @@ -1397,7 +1376,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) { } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args) +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1429,7 +1408,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPo return -1; } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args) +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args) { double dposXYZ[3]; dposXYZ[0] = textPosition.x(); @@ -1439,7 +1418,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &tex return addUserDebugText(text, &dposXYZ[0], args); } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1467,7 +1446,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *t return -1; } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) { double dfromXYZ[3]; double dtoXYZ[3]; @@ -1483,7 +1462,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(btVector3 &fromXYZ, btVect -bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args) +bool b3RobotSimulatorClientAPI_NoDirect::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1565,7 +1544,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId } -bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args) +bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1636,7 +1615,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi } -bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) +bool b3RobotSimulatorClientAPI_NoDirect::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1652,7 +1631,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int return true; } -bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags) +bool b3RobotSimulatorClientAPI_NoDirect::applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags) { double dforce[3]; double dposition[3]; @@ -1669,7 +1648,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int } -bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) +bool b3RobotSimulatorClientAPI_NoDirect::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1685,7 +1664,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, in return true; } -bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags) +bool b3RobotSimulatorClientAPI_NoDirect::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags) { double dtorque[3]; @@ -1697,7 +1676,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, in } -bool b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable) +bool b3RobotSimulatorClientAPI_NoDirect::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1723,7 +1702,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniq return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo) +bool b3RobotSimulatorClientAPI_NoDirect::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1744,7 +1723,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVi return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo) +bool b3RobotSimulatorClientAPI_NoDirect::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1780,7 +1759,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGe return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo) +bool b3RobotSimulatorClientAPI_NoDirect::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1815,7 +1794,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGe } -bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData) +bool b3RobotSimulatorClientAPI_NoDirect::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1834,7 +1813,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, dou } -bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData) +bool b3RobotSimulatorClientAPI_NoDirect::getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData) { double daabbMin[3]; double daabbMax[3]; @@ -1852,7 +1831,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(btVector3 &aabbMin, -bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax) +bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1891,7 +1870,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, d return false; } -bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax) +bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax) { double daabbMin[3]; double daabbMax[3]; @@ -1910,7 +1889,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, b } -int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args) +int b3RobotSimulatorClientAPI_NoDirect::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1962,7 +1941,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct return -1; } -int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args) +int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -2035,7 +2014,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCrea return -1; } -int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const +int b3RobotSimulatorClientAPI_NoDirect::getNumConstraints() const { if (!isConnected()) { b3Warning("Not connected"); @@ -2044,7 +2023,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const return b3GetNumUserConstraints(m_data->m_physicsClientHandle); } -int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex) +int b3RobotSimulatorClientAPI_NoDirect::getConstraintUniqueId(int serialIndex) { if (!isConnected()) { b3Warning("Not connected"); @@ -2057,17 +2036,24 @@ int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex) -void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* guiHelper) +void b3RobotSimulatorClientAPI_NoDirect::setGuiHelper(struct GUIHelperInterface* guiHelper) { m_data->m_guiHelper = guiHelper; } -struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper() + +struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoDirect::getGuiHelper() { return m_data->m_guiHelper; } -bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, int linkIndex, +void b3RobotSimulatorClientAPI_NoDirect::setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data) +{ + *m_data = *data; +} + + +bool b3RobotSimulatorClientAPI_NoDirect::getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; @@ -2092,7 +2078,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, in return true; } -bool b3RobotSimulatorClientAPI_NoGUI::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) +bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h similarity index 97% rename from examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h rename to examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 2a3dcc328..62b7a0272 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -1,5 +1,9 @@ -#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H -#define B3_ROBOT_SIMULATOR_CLIENT_API_H +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H + +///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet +///as documented in the pybullet Quickstart Guide +///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA #include "SharedMemoryPublic.h" #include "LinearMath/btVector3.h" @@ -404,11 +408,7 @@ struct b3RobotSimulatorCreateMultiBodyArgs }; - -///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_NoGUI +class b3RobotSimulatorClientAPI_NoDirect { protected: @@ -416,8 +416,8 @@ protected: public: - b3RobotSimulatorClientAPI_NoGUI(); - virtual ~b3RobotSimulatorClientAPI_NoGUI(); + b3RobotSimulatorClientAPI_NoDirect(); + virtual ~b3RobotSimulatorClientAPI_NoDirect(); bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); @@ -580,6 +580,10 @@ public: bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); + virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data); + }; -#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H + + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp new file mode 100644 index 000000000..6ccd8c274 --- /dev/null +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -0,0 +1,112 @@ +#include "b3RobotSimulatorClientAPI_NoGUI.h" + +#include "PhysicsClientC_API.h" +#include "b3RobotSimulatorClientAPI_InternalData.h" + +#ifdef BT_ENABLE_ENET +#include "PhysicsClientUDP_C_API.h" +#endif //PHYSICS_UDP + +#ifdef BT_ENABLE_CLSOCKET +#include "PhysicsClientTCP_C_API.h" +#endif //PHYSICS_TCP + +#ifndef BT_DISABLE_PHYSICS_DIRECT +#include "PhysicsDirectC_API.h" +#endif //BT_DISABLE_PHYSICS_DIRECT + +#include "SharedMemoryPublic.h" +#include "Bullet3Common/b3Logging.h" + + + +b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI() +{ +} + +b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI() +{ +} + + +bool b3RobotSimulatorClientAPI_NoGUI::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_DIRECT: + { +#ifndef BT_DISABLE_PHYSICS_DIRECT + sm = b3ConnectPhysicsDirect(); +#endif //BT_DISABLE_PHYSICS_DIRECT + + 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; +} diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h new file mode 100644 index 000000000..cca8a22a2 --- /dev/null +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h @@ -0,0 +1,22 @@ +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_H + +#include "b3RobotSimulatorClientAPI_NoDirect.h" + + +///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_NoGUI : public b3RobotSimulatorClientAPI_NoDirect +{ + +public: + + b3RobotSimulatorClientAPI_NoGUI(); + virtual ~b3RobotSimulatorClientAPI_NoGUI(); + + bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + +}; + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp new file mode 100644 index 000000000..d61937271 --- /dev/null +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp @@ -0,0 +1,98 @@ + +//tinyRendererPlugin implements the TinyRenderer as a plugin +//it is statically linked when using preprocessor #define STATIC_LINK_VR_PLUGIN +//otherwise you can dynamically load it using pybullet.loadPlugin + +#include "collisionFilterPlugin.h" +#include "../../SharedMemoryPublic.h" +#include "../b3PluginContext.h" +#include + +struct MyClass +{ + int m_testData; + + MyClass() + :m_testData(42) + { + } + virtual ~MyClass() + { + } +}; + +B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context) +{ + MyClass* obj = new MyClass(); + context->m_userPointer = obj; + + printf("hi!\n"); + return SHARED_MEMORY_MAGIC_NUMBER; +} + + +B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context) +{ + //apply pd control here, apply forces using the PD gains + return 0; +} + + +B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass* )context->m_userPointer; + obj->m_testData++; + return 0; +} + +B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +{ + //set the PD gains + printf("text argument:%s\n",arguments->m_text); + printf("int args: ["); + for (int i=0;im_numInts;i++) + { + printf("%d", arguments->m_ints[i]); + if ((i+1)m_numInts) + { + printf(","); + } + } + printf("]\nfloat args: ["); + for (int i=0;im_numFloats;i++) + { + printf("%f", arguments->m_floats[i]); + if ((i+1)m_numFloats) + { + printf(","); + } + } + printf("]\n"); + + MyClass* obj = (MyClass*) context->m_userPointer; + + b3SharedMemoryStatusHandle statusHandle; + int statusType = -1; + int bodyUniqueId = -1; + + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text); + + statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + bodyUniqueId = b3GetStatusBodyIndex(statusHandle); + } + return bodyUniqueId; +} + + +B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass*) context->m_userPointer; + delete obj; + context->m_userPointer = 0; + + printf("bye!\n"); +} diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h new file mode 100644 index 000000000..5fd96fd46 --- /dev/null +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h @@ -0,0 +1,25 @@ +#ifndef COLLISION_FILTER_PLUGIN_H +#define COLLISION_FILTER_PLUGIN_H + +#include "../b3PluginAPI.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +//the following 3 APIs are required +B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context); +B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context); +B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments); + +//all the APIs below are optional +B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context); +B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context); + + +#ifdef __cplusplus +}; +#endif + +#endif//#define COLLISION_FILTER_PLUGIN_H diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/premake4.lua b/examples/SharedMemory/plugins/collisionFilterPlugin/premake4.lua new file mode 100644 index 000000000..292447fb7 --- /dev/null +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/premake4.lua @@ -0,0 +1,42 @@ + + +project ("pybullet_collisionFilterPlugin") + language "C++" + kind "SharedLib" + + includedirs {".","../../../../src", "../../../../examples", + "../../../ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + hasCL = findOpenCL("clew") + + links{"BulletFileLoader", "Bullet3Common", "LinearMath"} + + + if os.is("MacOSX") then +-- targetextension {"so"} + links{"Cocoa.framework","Python"} + end + + + files { + "collisionFilterPlugin.cpp", + "../../PhysicsClient.cpp", + "../../PhysicsClient.h", + "../../PhysicsClientSharedMemory.cpp", + "../../PhysicsClientSharedMemory.h", + "../../PhysicsClientSharedMemory_C_API.cpp", + "../../PhysicsClientSharedMemory_C_API.h", + "../../PhysicsClientC_API.cpp", + "../../PhysicsClientC_API.h", + "../../Win32SharedMemory.cpp", + "../../Win32SharedMemory.h", + "../../PosixSharedMemory.cpp", + "../../PosixSharedMemory.h", + "../../../Utils/b3Clock.cpp", + "../../../Utils/b3Clock.h", + "../../../Utils/b3ResourcePath.cpp", + "../../../Utils/b3ResourcePath.h", + } + + + diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp new file mode 100644 index 000000000..7fd1c0913 --- /dev/null +++ b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp @@ -0,0 +1,162 @@ + +#include "pdControlPlugin.h" +#include "../../SharedMemoryPublic.h" +#include "../b3PluginContext.h" +#include + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" + +#include "../../b3RobotSimulatorClientAPI_NoDirect.h" +#include "../../b3RobotSimulatorClientAPI_InternalData.h" + +struct MyPDControl +{ + int m_objectUniqueId; + int m_linkIndex; + btScalar m_desiredPosition; + btScalar m_desiredVelocity; + btScalar m_kd; + btScalar m_kp; + btScalar m_maxForce; +}; + +struct MyPDControlContainer +{ + int m_testData; + btAlignedObjectArray m_controllers; + b3RobotSimulatorClientAPI_NoDirect m_api; + MyPDControlContainer() + :m_testData(42) + { + } + virtual ~MyPDControlContainer() + { + } +}; + +B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context) +{ + MyPDControlContainer* obj = new MyPDControlContainer(); + b3RobotSimulatorClientAPI_InternalData data; + data.m_physicsClientHandle = context->m_physClient; + data.m_guiHelper = 0; + obj->m_api.setInternalData(&data); + context->m_userPointer = obj; + + return SHARED_MEMORY_MAGIC_NUMBER; +} + + +B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context) +{ + //apply pd control here, apply forces using the PD gains + MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer; + for (int i = 0; i < obj->m_controllers.size(); i++) + { + const MyPDControl& pdControl = obj->m_controllers[i]; + //compute torque + //apply torque + } + //for each registered pd controller, execute PD control + + return 0; +} + + +B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context) +{ + MyPDControlContainer* obj = (MyPDControlContainer* )context->m_userPointer; + obj->m_testData++; + return 0; +} + +B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +{ + MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer; + + int numObj = obj->m_api.getNumBodies(); + printf("numObj = %d\n", numObj); + + //protocol: + //first int is the type of command + //second int is body unique id + //third int is link index + + if (arguments->m_numInts != 3) + return -1; + + + switch (arguments->m_ints[0]) + { + case eAddPDControl: + { + if (arguments->m_numFloats < 5) + return -1; + MyPDControl controller; + controller.m_desiredPosition = arguments->m_floats[0]; + controller.m_desiredVelocity = arguments->m_floats[1]; + controller.m_kd = arguments->m_floats[2]; + controller.m_kp = arguments->m_floats[3]; + controller.m_maxForce = arguments->m_floats[4]; + controller.m_objectUniqueId = arguments->m_ints[1]; + controller.m_linkIndex = arguments->m_ints[2]; + obj->m_controllers.push_back(controller); + } + case eSetPDControl: + { + if (arguments->m_numFloats < 5) + return -1; + MyPDControl controller; + controller.m_desiredPosition = arguments->m_floats[0]; + controller.m_desiredVelocity = arguments->m_floats[1]; + controller.m_kd = arguments->m_floats[2]; + controller.m_kp = arguments->m_floats[3]; + controller.m_maxForce = arguments->m_floats[4]; + controller.m_objectUniqueId = arguments->m_ints[1]; + controller.m_linkIndex = arguments->m_ints[2]; + + for (int i = 0; i < obj->m_controllers.size(); i++) + { + if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex) + { + obj->m_controllers[i] = controller; + break; + } + } + break; + } + case eRemovePDControl: + { + MyPDControl controller; + controller.m_objectUniqueId = arguments->m_ints[1]; + controller.m_linkIndex = arguments->m_ints[2]; + + for (int i = 0; i < obj->m_controllers.size(); i++) + { + if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex) + { + obj->m_controllers.removeAtIndex(i); + break; + } + } + break; + } + default: + { + return -1; + } + } + + int result = 42; + return result; +} + + +B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context) +{ + MyPDControlContainer* obj = (MyPDControlContainer*) context->m_userPointer; + delete obj; + context->m_userPointer = 0; + +} diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h new file mode 100644 index 000000000..591db0873 --- /dev/null +++ b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h @@ -0,0 +1,33 @@ +#ifndef PID_CONTROL_PLUGIN_H +#define PID_CONTROL_PLUGIN_H + +#include "../b3PluginAPI.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +//the following 3 APIs are required +B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context); +B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context); +B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments); + +/// +enum PDControlCommandEnum +{ + eAddPDControl = 1, + eSetPDControl = 2, + eRemovePDControl = 4, +}; + +//all the APIs below are optional +B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context); +B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context); + + +#ifdef __cplusplus +}; +#endif + +#endif//#define PID_CONTROL_PLUGIN_H diff --git a/examples/SharedMemory/plugins/pdControlPlugin/premake4.lua b/examples/SharedMemory/plugins/pdControlPlugin/premake4.lua new file mode 100644 index 000000000..4b52cacdd --- /dev/null +++ b/examples/SharedMemory/plugins/pdControlPlugin/premake4.lua @@ -0,0 +1,44 @@ + + +project ("pybullet_pdControlPlugin") + language "C++" + kind "SharedLib" + + includedirs {".","../../../../src", "../../../../examples", + "../../../ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + hasCL = findOpenCL("clew") + + links{"BulletFileLoader", "Bullet3Common", "LinearMath"} + + + if os.is("MacOSX") then +-- targetextension {"so"} + links{"Cocoa.framework","Python"} + end + + + files { + "pdControlPlugin.cpp", + "../../b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../b3RobotSimulatorClientAPI_NoDirect.h", + "../../PhysicsClient.cpp", + "../../PhysicsClient.h", + "../../PhysicsClientSharedMemory.cpp", + "../../PhysicsClientSharedMemory.h", + "../../PhysicsClientSharedMemory_C_API.cpp", + "../../PhysicsClientSharedMemory_C_API.h", + "../../PhysicsClientC_API.cpp", + "../../PhysicsClientC_API.h", + "../../Win32SharedMemory.cpp", + "../../Win32SharedMemory.h", + "../../PosixSharedMemory.cpp", + "../../PosixSharedMemory.h", + "../../../Utils/b3Clock.cpp", + "../../../Utils/b3Clock.h", + "../../../Utils/b3ResourcePath.cpp", + "../../../Utils/b3ResourcePath.h", + } + + + diff --git a/examples/SharedMemory/plugins/testPlugin/testplugin.cpp b/examples/SharedMemory/plugins/testPlugin/testplugin.cpp index 2ba2b6bf2..8c384edaf 100644 --- a/examples/SharedMemory/plugins/testPlugin/testplugin.cpp +++ b/examples/SharedMemory/plugins/testPlugin/testplugin.cpp @@ -29,7 +29,7 @@ struct MyClass } }; -B3_SHARED_API int initPlugin(struct b3PluginContext* context) +B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context) { MyClass* obj = new MyClass(); context->m_userPointer = obj; @@ -39,67 +39,23 @@ B3_SHARED_API int initPlugin(struct b3PluginContext* context) } -B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) +B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context) { - MyClass* obj = (MyClass* )context->m_userPointer; - { - b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient); - int deviceTypeFilter = VR_DEVICE_CONTROLLER; - b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); - - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); - int statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED) - { - struct b3VREventsData vrEvents; - - int i = 0; - b3GetVREventsData(context->m_physClient, &vrEvents); - if (vrEvents.m_numControllerEvents) - { - //this is only for a test, normally you wouldn't print to the console at each simulation substep! - printf("got %d VR controller events!\n", vrEvents.m_numControllerEvents); - } - } - } - { - b3KeyboardEventsData keyboardEventsData; - b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(context->m_physClient); - b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); - b3GetKeyboardEventsData(context->m_physClient, &keyboardEventsData); - if (keyboardEventsData.m_numKeyboardEvents) - { - //this is only for a test, normally you wouldn't print to the console at each simulation substep! - printf("got %d keyboard events\n", keyboardEventsData.m_numKeyboardEvents); - } - } - - { - b3MouseEventsData mouseEventsData; - b3SharedMemoryCommandHandle commandHandle = b3RequestMouseEventsCommandInit(context->m_physClient); - b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); - b3GetMouseEventsData(context->m_physClient, &mouseEventsData); - if (mouseEventsData.m_numMouseEvents) - { - //this is only for a test, normally you wouldn't print to the console at each simulation substep! - printf("got %d mouse events\n", mouseEventsData.m_numMouseEvents); - } - } - return 0; } -B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context) +B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context) { MyClass* obj = (MyClass* )context->m_userPointer; obj->m_testData++; return 0; } -B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { + printf("text argument:%s\n",arguments->m_text); printf("int args: ["); for (int i=0;im_numInts;i++) @@ -140,7 +96,7 @@ B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const st } -B3_SHARED_API void exitPlugin(struct b3PluginContext* context) +B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context) { MyClass* obj = (MyClass*) context->m_userPointer; delete obj; diff --git a/examples/SharedMemory/plugins/testPlugin/testplugin.h b/examples/SharedMemory/plugins/testPlugin/testplugin.h index 433634762..cf6e6f46e 100644 --- a/examples/SharedMemory/plugins/testPlugin/testplugin.h +++ b/examples/SharedMemory/plugins/testPlugin/testplugin.h @@ -9,14 +9,13 @@ extern "C" #endif //initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load -B3_SHARED_API int initPlugin(struct b3PluginContext* context); -B3_SHARED_API void exitPlugin(struct b3PluginContext* context); -B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments); +B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context); +B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context); +B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments); //all the APIs below are optional -B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context); -B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context); -B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface(struct b3PluginContext* context); +B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context); +B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context); #ifdef __cplusplus diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index e211f6d38..47f8be860 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -465,4 +465,7 @@ include "plugins/testPlugin" include "plugins/vrSyncPlugin" include "plugins/tinyRendererPlugin" +include "plugins/pdControlPlugin" +include "plugins/collisionFilterPlugin" + diff --git a/examples/pybullet/examples/renderPlugin.py b/examples/pybullet/examples/renderPlugin.py index 91c188fbf..48dfc8aa6 100644 --- a/examples/pybullet/examples/renderPlugin.py +++ b/examples/pybullet/examples/renderPlugin.py @@ -1,7 +1,7 @@ import pybullet as p p.connect(p.GUI) -plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_tinyRendererPlugin_vs2010_x64_debug.dll","_tinyRendererPlugin") +plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin") print("plugin=",plugin) p.loadURDF("r2d2.urdf") diff --git a/examples/pybullet/examples/testPlugin.py b/examples/pybullet/examples/testPlugin.py new file mode 100644 index 000000000..5bc2e22ed --- /dev/null +++ b/examples/pybullet/examples/testPlugin.py @@ -0,0 +1,10 @@ + +import pybullet as p +p.connect(p.GUI) +plugin = p.loadPlugin("D:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin") +print("plugin=",plugin) +p.executePluginCommand(plugin ,"r2d2.urdf", [1,2,3],[50.0,3.3]) + +while (1): + p.getCameraImage(320,200) + p.stepSimulation() \ No newline at end of file