diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 8abac18f5..b2c4523b6 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -120,6 +120,10 @@ ELSE(WIN32) IF(BUILD_CLSOCKET) ADD_DEFINITIONS(${OSDEF}) ENDIF(BUILD_CLSOCKET) + + IF(NOT APPLE) + LINK_LIBRARIES( pthread ${DL} ) + ENDIF(APPLE) ENDIF(WIN32) IF(BUILD_ENET) diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 2554ac4fa..aec69df37 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -175,6 +175,8 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/b3PluginManager.cpp ../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 56789385c..65e38ece8 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -122,6 +122,8 @@ 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/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index d0bb13eaf..0647edf72 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -1,4 +1,3 @@ - INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/examples @@ -9,77 +8,20 @@ INCLUDE_DIRECTORIES( 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/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp - ../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp - ../../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/PhysicsServerExampleBullet2.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/b3PluginManager.cpp - - ../../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 - -) +RobotSimulatorMain.cpp + b3RobotSimulatorClientAPI.cpp + b3RobotSimulatorClientAPI.h + b3RobotSimulatorClientAPI_NoGUI.cpp + b3RobotSimulatorClientAPI_NoGUI.h + MinitaurSetup.cpp + MinitaurSetup.h + ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/PhysicsServerExample.cpp + ../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp + ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +) + + IF(BUILD_CLSOCKET) ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET) @@ -89,54 +31,9 @@ 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) - ADD_DEFINITIONS(${OSDEF}) - 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() - #some code to support OpenGL and Glew cross platform IF (WIN32) @@ -168,6 +65,8 @@ ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS}) SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION}) SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d") +SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES COMPILE_DEFINITIONS "B3_USE_ROBOTSIM_GUI") + IF(WIN32) @@ -176,8 +75,56 @@ IF(WIN32) ENDIF(BUILD_ENET OR BUILD_CLSOCKET) ENDIF(WIN32) +TARGET_LINK_LIBRARIES(App_RobotSimulator BulletRobotics BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common) + + + + + + + +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_NoGUI_SRCS + RobotSimulatorMain.cpp + b3RobotSimulatorClientAPI_NoGUI.cpp + b3RobotSimulatorClientAPI_NoGUI.h + MinitaurSetup.cpp + MinitaurSetup.h +) + +ADD_EXECUTABLE(App_RobotSimulator_NoGUI ${RobotSimulator_NoGUI_SRCS}) + +SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES DEBUG_POSTFIX "_d") + + +IF(WIN32) + IF(BUILD_ENET OR BUILD_CLSOCKET) + TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI ws2_32 ) + ENDIF(BUILD_ENET OR BUILD_CLSOCKET) +ELSE() + IF(APPLE) + ELSE(APPLE) + TARGET_LINK_LIBRARIES( App_RobotSimulator_NoGUI pthread ${DL} ) + ENDIF(APPLE) +ENDIF(WIN32) + +TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath Bullet3Common) + + + + + + + -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 index ec4a174e4..16d102072 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -1,5 +1,5 @@ #include "MinitaurSetup.h" -#include "b3RobotSimulatorClientAPI.h" +#include "b3RobotSimulatorClientAPI_NoGUI.h" #include "Bullet3Common/b3HashMap.h" @@ -27,7 +27,7 @@ MinitaurSetup::~MinitaurSetup() delete m_data; } -void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd) +void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); controlArgs.m_maxTorqueValue = maxTorque; @@ -158,7 +158,7 @@ static const char* minitaurURDF="quadruped/minitaur_rainbow_dash_v1.urdf"; #endif -void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) +void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim) { //release all motors int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); @@ -255,7 +255,7 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) } -int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) +int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) { b3RobotSimulatorLoadUrdfFileArgs args; diff --git a/examples/RobotSimulator/MinitaurSetup.h b/examples/RobotSimulator/MinitaurSetup.h index a054ade92..6d9f8c211 100644 --- a/examples/RobotSimulator/MinitaurSetup.h +++ b/examples/RobotSimulator/MinitaurSetup.h @@ -6,15 +6,15 @@ class MinitaurSetup { struct MinitaurSetupInternalData* m_data; - void resetPose(class b3RobotSimulatorClientAPI* sim); + void resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim); 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)); + int setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1)); - void setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9); + void setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9); }; #endif //MINITAUR_SIMULATION_SETUP_H diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index c3711f20f..2a6b90584 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -1,5 +1,11 @@ -#include "b3RobotSimulatorClientAPI.h" +#ifdef B3_USE_ROBOTSIM_GUI + #include "b3RobotSimulatorClientAPI.h" +#else + #include "b3RobotSimulatorClientAPI_NoGUI.h" +#endif + + #include "../Utils/b3Clock.h" #include @@ -7,11 +13,23 @@ #include #define ASSERT_EQ(a,b) assert((a)==(b)); #include "MinitaurSetup.h" + + + int main(int argc, char* argv[]) { +#ifdef B3_USE_ROBOTSIM_GUI b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI(); - - sim->connect(eCONNECT_GUI); + bool isConnected = sim->connect(eCONNECT_GUI); +#else + b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI(); + bool isConnected = sim->connect(eCONNECT_DIRECT); +#endif + if (!isConnected) + { + printf("Cannot connect\n"); + return -1; + } //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); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 1a7bf5b77..f22e4c0aa 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -1,9 +1,7 @@ #include "b3RobotSimulatorClientAPI.h" -//#include "SharedMemoryCommands.h" - #include "../SharedMemory/PhysicsClientC_API.h" - +#include "b3RobotSimulatorClientAPI_InternalData.h"" #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //PHYSICS_UDP @@ -16,36 +14,20 @@ #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" - #include "../SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" -struct b3RobotSimulatorClientAPI_InternalData -{ - b3PhysicsClientHandle m_physicsClientHandle; - struct GUIHelperInterface* m_guiHelper; - - b3RobotSimulatorClientAPI_InternalData() - : m_physicsClientHandle(0), - m_guiHelper(0) - { - } -}; b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI() { - m_data = new b3RobotSimulatorClientAPI_InternalData(); + } b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() { - delete m_data; } -void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper) -{ - m_data->m_guiHelper = guiHelper; -} + void b3RobotSimulatorClientAPI::renderScene() { @@ -82,7 +64,7 @@ bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y) } if (m_data->m_guiHelper) { - return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y); + return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y)!=0; } return false; } @@ -95,7 +77,7 @@ bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float } if (m_data->m_guiHelper) { - return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y); + return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y)!=0; } return false; } @@ -118,13 +100,13 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i switch (mode) { - case eCONNECT_EXISTING_EXAMPLE_BROWSER: + case eCONNECT_EXISTING_EXAMPLE_BROWSER: { sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper); break; } - case eCONNECT_GUI: + case eCONNECT_GUI: { int argc = 0; char* argv[1] = {0}; @@ -135,7 +117,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i #endif break; } - case eCONNECT_GUI_SERVER: + case eCONNECT_GUI_SERVER: { int argc = 0; char* argv[1] = {0}; @@ -146,12 +128,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i #endif break; } - case eCONNECT_DIRECT: + case eCONNECT_DIRECT: { sm = b3ConnectPhysicsDirect(); break; } - case eCONNECT_SHARED_MEMORY: + case eCONNECT_SHARED_MEMORY: { if (portOrKey >= 0) { @@ -160,7 +142,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i sm = b3ConnectSharedMemory(key); break; } - case eCONNECT_UDP: + case eCONNECT_UDP: { if (portOrKey >= 0) { @@ -175,7 +157,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i break; } - case eCONNECT_TCP: + case eCONNECT_TCP: { if (portOrKey >= 0) { @@ -190,7 +172,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i break; } - default: + default: { b3Warning("connectPhysicsServer unexpected argument"); } @@ -208,952 +190,3 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i } return false; } - -bool b3RobotSimulatorClientAPI::isConnected() const -{ - return (m_data->m_physicsClientHandle != 0); -} - -void b3RobotSimulatorClientAPI::setTimeOut(double timeOutInSec) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec); - -} - - -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)); -} - -bool b3RobotSimulatorClientAPI::canSubmitCommand() const -{ - if (!isConnected()) - { - return false; - } - return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); -} - -void b3RobotSimulatorClientAPI::stepSimulation() -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - 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; - } - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); - - 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) -{ - b3Quaternion q; - q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); - return q; -} - -b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) -{ - b3Scalar roll,pitch,yaw; - quat.getEulerZYX(yaw,pitch,roll); - b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); - return rpy2; -} - -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 - - b3LoadUrdfCommandSetFlags(command,args.m_flags); - - 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) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - 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::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); - return (result != 0); -} - -bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - 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) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - 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; -} - -bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - const int status_type = b3GetStatusType(statusHandle); - const double* actualStateQdot; - - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - return false; - } - - b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, 0, - &actualStateQdot, 0 /* joint_reaction_forces */); - - baseLinearVelocity[0] = actualStateQdot[0]; - baseLinearVelocity[1] = actualStateQdot[1]; - baseLinearVelocity[2] = actualStateQdot[2]; - - baseAngularVelocity[0] = actualStateQdot[3]; - baseAngularVelocity[1] = actualStateQdot[4]; - baseAngularVelocity[2] = actualStateQdot[5]; - return true; -} - -bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - - commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - - b3Vector3DoubleData linVelDouble; - linearVelocity.serializeDouble(linVelDouble); - b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); - - b3Vector3DoubleData angVelDouble; - angularVelocity.serializeDouble(angVelDouble); - b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); - - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - return true; -} - -void b3RobotSimulatorClientAPI::setInternalSimFlags(int flags) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - { - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetInternalSimFlags(command, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - } -} - - -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); -} - -int b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return -1; - } - 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)); - int statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_CONSTRAINT_COMPLETED) - { - int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); - return userConstraintUid; - } - } - return -1; -} - -int b3RobotSimulatorClientAPI::changeConstraint(int constraintId, b3JointInfo* jointInfo) -{ - - if (!isConnected()) - { - b3Warning("Not connected"); - return -1; - } - b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); - - if (jointInfo->m_flags & eJointChangeMaxForce) - { - b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce); - } - - if (jointInfo->m_flags & eJointChangeChildFramePosition) - { - b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]); - } - if (jointInfo->m_flags & eJointChangeChildFrameOrientation) - { - b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]); - } - - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - int statusType = b3GetStatusType(statusHandle); - return statusType; -} - -void b3RobotSimulatorClientAPI::removeConstraint(int constraintId) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - int statusType = b3GetStatusType(statusHandle); -} - - -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::getJointStates(int bodyUniqueId, b3JointStates2& state) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return false; - } - - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (statusHandle) - { - // double rootInertialFrame[7]; - const double* rootLocalInertialFrame; - const double* actualStateQ; - const double* actualStateQdot; - const double* jointReactionForces; - - int stat = b3GetStatusActualState(statusHandle, - &state.m_bodyUniqueId, - &state.m_numDegreeOfFreedomQ, - &state.m_numDegreeOfFreedomU, - &rootLocalInertialFrame, - &actualStateQ, - &actualStateQdot, - &jointReactionForces); - if (stat) - { - state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); - state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); - - for (int i=0;im_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::setContactBreakingThreshold(double threshold) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetContactBreakingThreshold(command,threshold); - 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) - { - int dofCount; - b3GetStatusJacobian(statusHandle, &dofCount, 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); -} - -void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) -{ - vrEventsData->m_numControllerEvents = 0; - vrEventsData->m_controllerEvents = 0; - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); - b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); -} - -void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) -{ - keyboardEventsData->m_numKeyboardEvents = 0; - keyboardEventsData->m_keyboardEvents = 0; - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); -} - -int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return -1; - } - int loggingUniqueId = -1; - b3SharedMemoryCommandHandle commandHandle; - commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); - - b3StateLoggingStart(commandHandle, loggingType, fileName.c_str()); - - for (int i = 0; i < objectUniqueIds.size(); i++) - { - int objectUid = objectUniqueIds[i]; - b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid); - } - - if (maxLogDof > 0) - { - b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); - } - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_STATE_LOGGING_START_COMPLETED) - { - loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); - } - return loggingUniqueId; -} - -void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); - b3StateLoggingStop(commandHandle, stateLoggerUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); -} - -void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); - if (commandHandle) - { - if ((cameraDistance >= 0)) - { - b3Vector3FloatData camTargetPos; - targetPos.serializeFloat(camTargetPos); - b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); - } - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - } -} - -void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str()); - if (durationInMicroSeconds>=0) - { - b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds); - } - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); -} - -void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin) -{ - if (!isConnected()) - { - b3Warning("Not connected"); - return; - } - - b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSoftBodySetScale(command, scale); - b3LoadSoftBodySetMass(command, mass); - b3LoadSoftBodySetCollisionMargin(command, collisionMargin); - b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); -} diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index d2bee5338..a5f1a0cbd 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -1,246 +1,32 @@ -#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H -#define B3_ROBOT_SIMULATOR_CLIENT_API_H +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H -#include "../SharedMemory/SharedMemoryPublic.h" -#include "Bullet3Common/b3Vector3.h" -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3Transform.h" -#include "Bullet3Common/b3AlignedObjectArray.h" +#include "b3RobotSimulatorClientAPI_NoGUI.h" -#include -struct b3RobotSimulatorLoadUrdfFileArgs -{ - b3Vector3 m_startPosition; - b3Quaternion m_startOrientation; - bool m_forceOverrideFixedBase; - bool m_useMultiBody; - int m_flags; - - b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) - : m_startPosition(startPos), - m_startOrientation(startOrn), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) - { - } - - b3RobotSimulatorLoadUrdfFileArgs() - : m_startPosition(b3MakeVector3(0, 0, 0)), - m_startOrientation(b3Quaternion(0, 0, 0, 1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true), - m_flags(0) - { - } -}; - -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; -}; - -struct b3JointStates2 -{ - int m_bodyUniqueId; - int m_numDegreeOfFreedomQ; - int m_numDegreeOfFreedomU; - b3Transform m_rootLocalInertialFrame; - b3AlignedObjectArray m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray m_jointReactionForces; -}; - -///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet +///The b3RobotSimulatorClientAPI_GUI is pretty much the C++ version of pybullet ///as documented in the pybullet Quickstart Guide ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA -class b3RobotSimulatorClientAPI +class b3RobotSimulatorClientAPI : public b3RobotSimulatorClientAPI_NoGUI { - 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 setTimeOut(double timeOutInSec); - - 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 getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); - - bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; - bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); - - bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; - bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; - - int getNumJoints(int bodyUniqueId) const; - - bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); - - int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); - - int changeConstraint(int constraintId, b3JointInfo* jointInfo); - - void removeConstraint(int constraintId); - - bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); - - bool getJointStates(int bodyUniqueId, b3JointStates2& state); - - bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); - - void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); - - void stepSimulation(); - - bool canSubmitCommand() const; - - void setRealTimeSimulation(bool enableRealTimeSimulation); - - void setInternalSimFlags(int flags); - - void setGravity(const b3Vector3& gravityAcceleration); - - void setTimeStep(double timeStepInSeconds); - void setNumSimulationSubSteps(int numSubSteps); - void setNumSolverIterations(int numIterations); - void setContactBreakingThreshold(double threshold); - - 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); - void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos); - - int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); - void stopStateLogging(int stateLoggerUniqueId); - - void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); - void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); - - void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); - - - //////////////// INTERNAL - - void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin); - - //setGuiHelper is only used when embedded in existing example browser - void setGuiHelper(struct GUIHelperInterface* guiHelper); - //renderScene is only used when embedded in existing example browser + virtual bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + virtual void renderScene(); - //debugDraw is only used when embedded in existing example browser + virtual void debugDraw(int debugDrawMode); + virtual bool mouseMoveCallback(float x,float y); + virtual bool mouseButtonCallback(int button, int state, float x, float y); - ////////////////INTERNAL }; #endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h new file mode 100644 index 000000000..cac680fda --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h @@ -0,0 +1,18 @@ +#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H +#define B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H + +#include "../SharedMemory/PhysicsClientC_API.h" + +struct b3RobotSimulatorClientAPI_InternalData +{ + b3PhysicsClientHandle m_physicsClientHandle; + struct GUIHelperInterface* m_guiHelper; + + b3RobotSimulatorClientAPI_InternalData() + : m_physicsClientHandle(0), + m_guiHelper(0) + { + } +}; + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp new file mode 100644 index 000000000..8beeed506 --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -0,0 +1,1997 @@ +#include "b3RobotSimulatorClientAPI_NoGUI.h" + + +#include "../SharedMemory/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 "Bullet3Common/b3Logging.h" + +static void scalarToDouble3(b3Scalar a[3], double b[3]) +{ + for (int i = 0; i < 3; i++) + { + b[i] = a[i]; + } +} + +static void scalarToDouble4(b3Scalar a[4], double b[4]) +{ + for (int i = 0; i < 4; i++) + { + b[i] = a[i]; + } +} + + +b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI() +{ + m_data = new b3RobotSimulatorClientAPI_InternalData(); +} + +b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI() +{ + delete m_data; +} + + +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: + { + 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_NoGUI::isConnected() const +{ + return (m_data->m_physicsClientHandle != 0); +} + +void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec); + +} + + +void b3RobotSimulatorClientAPI_NoGUI::disconnect() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3DisconnectSharedMemory(m_data->m_physicsClientHandle); + m_data->m_physicsClientHandle = 0; +} + +void b3RobotSimulatorClientAPI_NoGUI::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_NoGUI::resetSimulation() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); +} + +bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const +{ + if (!isConnected()) + { + return false; + } + return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); +} + +void b3RobotSimulatorClientAPI_NoGUI::stepSimulation() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + 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_NoGUI::setGravity(const b3Vector3& gravityAcceleration) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + + 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_NoGUI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) +{ + b3Quaternion q; + q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); + return q; +} + +b3Vector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const b3Quaternion& quat) +{ + b3Scalar roll,pitch,yaw; + quat.getEulerZYX(yaw,pitch,roll); + b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); + return rpy2; +} + +int b3RobotSimulatorClientAPI_NoGUI::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 + + b3LoadUrdfCommandSetFlags(command,args.m_flags); + + 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_NoGUI::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_NoGUI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + 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_NoGUI::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_NoGUI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); + return (result != 0); +} + +bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + 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_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + 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; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + const int status_type = b3GetStatusType(statusHandle); + const double* actualStateQdot; + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + return false; + } + + b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3Vector3DoubleData linVelDouble; + linearVelocity.serializeDouble(linVelDouble); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); + + b3Vector3DoubleData angVelDouble; + angularVelocity.serializeDouble(angVelDouble); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + return true; +} + +void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetInternalSimFlags(command, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + } +} + + +void b3RobotSimulatorClientAPI_NoGUI::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_NoGUI::getNumJoints(int bodyUniqueId) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); +} + +bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); +} + +int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + 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)); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_CONSTRAINT_COMPLETED) + { + int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); + return userConstraintUid; + } + } + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointInfo* jointInfo) +{ + + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); + + if (jointInfo->m_flags & eJointChangeMaxForce) + { + b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce); + } + + if (jointInfo->m_flags & eJointChangeChildFramePosition) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]); + } + if (jointInfo->m_flags & eJointChangeChildFrameOrientation) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]); + } + + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + int statusType = b3GetStatusType(statusHandle); + return statusType; +} + +void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + int statusType = b3GetStatusType(statusHandle); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::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_NoGUI::getJointStates(int bodyUniqueId, b3JointStates2& state) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (statusHandle) + { + // double rootInertialFrame[7]; + const double* rootLocalInertialFrame; + const double* actualStateQ; + const double* actualStateQdot; + const double* jointReactionForces; + + int stat = b3GetStatusActualState(statusHandle, + &state.m_bodyUniqueId, + &state.m_numDegreeOfFreedomQ, + &state.m_numDegreeOfFreedomU, + &rootLocalInertialFrame, + &actualStateQ, + &actualStateQdot, + &jointReactionForces); + if (stat) + { + state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); + state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); + + for (int i=0;im_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_NoGUI::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_NoGUI::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_NoGUI::setContactBreakingThreshold(double threshold) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetContactBreakingThreshold(command,threshold); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); +} + + +void b3RobotSimulatorClientAPI_NoGUI::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_NoGUI::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_NoGUI::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_NoGUI::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) + { + int dofCount; + b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian); + return true; + } + return false; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState) +{ + bool computeLinkVelocity = true; + bool computeForwardKinematics = true; + + return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState); +} + +bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + if (computeLinkVelocity) { + b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity); + } + + if (computeForwardKinematics) { + b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics); + } + + 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_NoGUI::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); +} + +void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter) +{ + vrEventsData->m_numControllerEvents = 0; + vrEventsData->m_controllerEvents = 0; + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); + b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); +} + +void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = 0; + keyboardEventsData->m_keyboardEvents = 0; + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); +} + +int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + int loggingUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); + + b3StateLoggingStart(commandHandle, loggingType, fileName.c_str()); + + for (int i = 0; i < objectUniqueIds.size(); i++) + { + int objectUid = objectUniqueIds[i]; + b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid); + } + + if (maxLogDof > 0) + { + b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_STATE_LOGGING_START_COMPLETED) + { + loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); + } + return loggingUniqueId; +} + +void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); + b3StateLoggingStop(commandHandle, stateLoggerUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + +void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); + if (commandHandle) + { + if ((cameraDistance >= 0)) + { + b3Vector3FloatData camTargetPos; + targetPos.serializeFloat(camTargetPos); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + } +} + +void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str()); + if (durationInMicroSeconds>=0) + { + b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); +} + +void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); + b3LoadSoftBodySetScale(command, scale); + b3LoadSoftBodySetMass(command, mass); + b3LoadSoftBodySetCollisionMargin(command, collisionMargin); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); +} + +void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = 0; + mouseEventsData->m_mouseEvents = 0; + if (!isConnected()) { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle); + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, + double *jointAccelerations, double *jointForcesOutput) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + + int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, + jointVelocities, jointAccelerations); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0); + + if (dofCount) { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput); + return true; + } + } + return false; +} + +int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + return b3GetNumBodies(m_data->m_physicsClientHandle); +} + +int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId); +} + +bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId)); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_REMOVE_BODY_COMPLETED) { + return true; + } else { + b3Warning("getDynamicsInfo did not complete"); + return false; + } + } + b3Warning("removeBody could not submit command"); + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) { + if (!isConnected()) { + b3Warning("Not connected"); + return false; + } + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + // struct b3DynamicsInfo info; + + if (bodyUniqueId < 0) { + b3Warning("getDynamicsInfo failed; invalid bodyUniqueId"); + return false; + } + if (linkIndex < -1) { + b3Warning("getDynamicsInfo failed; invalid linkIndex"); + return false; + } + + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) { + cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex); + status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) { + return true; + } else { + b3Warning("getDynamicsInfo did not complete"); + return false; + } + } + b3Warning("getDynamicsInfo could not submit command"); + return false; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (args.m_mass >= 0) { + b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass); + } + + if (args.m_lateralFriction >= 0) { + b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction); + } + + if (args.m_spinningFriction>=0) { + b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction); + } + + if (args.m_rollingFriction>=0) { + b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction); + } + + if (args.m_linearDamping>=0) { + b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping); + } + + if (args.m_angularDamping>=0) { + b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping); + } + + if (args.m_restitution>=0) { + b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution); + } + + if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) { + b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping); + } + + if (args.m_frictionAnchor>=0) { + b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugParameter failed."); + return -1; +} + + +double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return 0; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) { + double paramValue = 0.f; + int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue); + if (ok) { + return paramValue; + } + } + b3Warning("readUserDebugParameter failed."); + return 0; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) { + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + return true; +} + + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime); + + if (args.m_parentObjectUniqueId>=0) { + b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); + } + + if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) { + b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugText3D failed."); + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +{ + double dposXYZ[3]; + dposXYZ[0] = posXYZ.x; + dposXYZ[1] = posXYZ.y; + dposXYZ[2] = posXYZ.z; + + return addUserDebugText(text, &dposXYZ[0], args); +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return -1; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime); + + if (args.m_parentObjectUniqueId>=0) { + b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + return debugItemUniqueId; + } + b3Warning("addUserDebugLine failed."); + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args) +{ + double dfromXYZ[3]; + double dtoXYZ[3]; + dfromXYZ[0] = fromXYZ.x; + dfromXYZ[1] = fromXYZ.y; + dfromXYZ[2] = fromXYZ.z; + + dtoXYZ[0] = toXYZ.x; + dtoXYZ[1] = toXYZ.y; + dtoXYZ[2] = toXYZ.z; + return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args); +} + + + +bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected to physics server."); + return false; + } + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + // int statusType; + struct b3JointInfo info; + + commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode); + + for (int i=0;im_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (args.m_numSolverIterations >= 0) { + b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations); + } + + if (args.m_collisionFilterMode >= 0) { + b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode); + } + + if (args.m_numSubSteps >= 0) { + b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps); + } + + if (args.m_fixedTimeStep >= 0) { + b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep); + } + + if (args.m_useSplitImpulse >= 0) { + b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse); + } + + if (args.m_splitImpulsePenetrationThreshold >= 0) { + b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold); + } + + if (args.m_contactBreakingThreshold >= 0) { + b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold); + } + + if (args.m_maxNumCmdPer1ms >= -1) { + b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms); + } + + if (args.m_restitutionVelocityThreshold>=0) { + b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold); + } + + if (args.m_enableFileCaching>=0) { + b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching); + } + + if (args.m_erp>=0) { + b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp); + } + + if (args.m_contactERP>=0) { + b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP); + } + + if (args.m_frictionERP >=0) { + b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags) +{ + double dforce[3]; + double dposition[3]; + + dforce[0] = force.x; + dforce[1] = force.y; + dforce[2] = force.z; + + dposition[0] = position.x; + dposition[1] = position.y; + dposition[2] = position.z; + + return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags) +{ + double dtorque[3]; + + dtorque[0] = torque.x; + dtorque[1] = torque.y; + dtorque[2] = torque.z; + + return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags); +} + + +bool b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex < 0) || (jointIndex >= numJoints)) { + b3Warning("Error: invalid jointIndex."); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3CreateSensorCommandInit(sm, bodyUniqueId); + b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CLIENT_COMMAND_COMPLETED) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitRequestOpenGLVisualizerCameraCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo); + + if (statusType) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitRequestContactPointInformation(sm); + + if (args.m_bodyUniqueIdA>=0) { + b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA); + } + if (args.m_bodyUniqueIdB>=0) { + b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB); + } + if (args.m_linkIndexA>=-1) { + b3SetContactFilterLinkA(command, args.m_linkIndexA); + } + if (args.m_linkIndexB >=-1) { + b3SetContactFilterLinkB(command, args.m_linkIndexB); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + b3GetContactPointInformation(sm, contactInfo); + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + command = b3InitClosestDistanceQuery(sm); + + b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA); + b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB); + b3SetClosestDistanceThreshold(command, distance); + + if (args.m_linkIndexA>=-1) { + b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA); + } + if (args.m_linkIndexB >=-1) { + b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { + b3GetContactPointInformation(sm, contactInfo); + return true; + } + return false; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + // int statusType; + + command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + b3GetAABBOverlapResults(sm, overlapData); + + return true; +} + + +bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData) +{ + double daabbMin[3]; + double daabbMax[3]; + + daabbMin[0] = aabbMin.x; + daabbMin[1] = aabbMin.y; + daabbMin[2] = aabbMin.z; + + daabbMax[0] = aabbMax.x; + daabbMax[1] = aabbMax.y; + daabbMax[2] = aabbMax.z; + + return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData); +} + + + +bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (bodyUniqueId < 0) { + b3Warning("Invalid bodyUniqueId"); + return false; + } + + if (linkIndex < -1) { + b3Warning("Invalid linkIndex"); + return false; + } + + if (aabbMin == NULL || aabbMax == NULL) { + b3Warning("Output AABB matrix is NULL"); + return false; + } + + command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) { + return false; + } + if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) { + return true; + } + return false; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax) +{ + double daabbMin[3]; + double daabbMax[3]; + + bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]); + + aabbMin.x = (float)daabbMin[0]; + aabbMin.y = (float)daabbMin[1]; + aabbMin.z = (float)daabbMin[2]; + + aabbMax.x = (float)daabbMax[0]; + aabbMax.y = (float)daabbMax[1]; + aabbMax.z = (float)daabbMax[2]; + + return status; +} + + +int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int shapeIndex = -1; + + command = b3CreateCollisionShapeCommandInit(sm); + + if (shapeType==GEOM_SPHERE && args.m_radius>0) { + shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius); + } + if (shapeType==GEOM_BOX) { + double halfExtents[3]; + scalarToDouble3(args.m_halfExtents, halfExtents); + shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents); + } + if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) { + shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height); + } + if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) { + shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height); + } + if (shapeType==GEOM_MESH && args.m_fileName) { + double meshScale[3]; + scalarToDouble3(args.m_meshScale, meshScale); + shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale); + } + if (shapeType==GEOM_PLANE) { + double planeConstant=0; + double planeNormal[3]; + scalarToDouble3(args.m_planeNormal, planeNormal); + shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant); + } + if (shapeIndex>=0 && args.m_flags) { + b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) { + int uid = b3GetStatusCollisionShapeUniqueId(statusHandle); + return uid; + } + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType, baseIndex; + + double doubleBasePosition[3]; + double doubleBaseInertialFramePosition[3]; + scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition); + scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition); + + double doubleBaseOrientation[4]; + double doubleBaseInertialFrameOrientation[4]; + scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation); + scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation); + + command = b3CreateMultiBodyCommandInit(sm); + + baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex, + doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation); + + for (int i = 0; i < args.m_numLinks; i++) { + double linkMass = args.m_linkMasses[i]; + int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i]; + int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i]; + b3Vector3 linkPosition = args.m_linkPositions[i]; + b3Quaternion linkOrientation = args.m_linkOrientations[i]; + b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i]; + b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i]; + int linkParentIndex = args.m_linkParentIndices[i]; + int linkJointType = args.m_linkJointTypes[i]; + b3Vector3 linkJointAxis = args.m_linkJointAxes[i]; + + double doubleLinkPosition[3]; + double doubleLinkInertialFramePosition[3]; + double doubleLinkJointAxis[3]; + scalarToDouble3(linkPosition.m_floats, doubleLinkPosition); + scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition); + scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis); + + double doubleLinkOrientation[4]; + double doubleLinkInertialFrameOrientation[4]; + scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation); + scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation); + + b3CreateMultiBodyLink(command, + linkMass, + linkCollisionShapeIndex, + linkVisualShapeIndex, + doubleLinkPosition, + doubleLinkOrientation, + doubleLinkInertialFramePosition, + doubleLinkInertialFrameOrientation, + linkParentIndex, + linkJointType, + doubleLinkJointAxis + ); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) { + int uid = b3GetStatusBodyIndex(statusHandle); + return uid; + } + return -1; +} + +int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const +{ + if (!isConnected()) { + b3Warning("Not connected"); + return -1; + } + return b3GetNumUserConstraints(m_data->m_physicsClientHandle); +} + +int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex) +{ + if (!isConnected()) { + b3Warning("Not connected"); + return -1; + } + int userConstraintId = -1; + userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex); + return userConstraintId; +} + + + +void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* guiHelper) +{ + m_data->m_guiHelper = guiHelper; +} + +struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper() +{ + return m_data->m_guiHelper; +} \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h new file mode 100644 index 000000000..229d058ec --- /dev/null +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h @@ -0,0 +1,572 @@ +#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; + int m_flags; + + b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn) + : m_startPosition(startPos), + m_startOrientation(startOrn), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) + { + } + + b3RobotSimulatorLoadUrdfFileArgs() + : m_startPosition(b3MakeVector3(0, 0, 0)), + m_startOrientation(b3Quaternion(0, 0, 0, 1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true), + m_flags(0) + { + } +}; + +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; +}; + +struct b3JointStates2 +{ + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray m_jointReactionForces; +}; + + +struct b3RobotSimulatorJointMotorArrayArgs +{ + int m_controlMode; + int m_numControlledDofs; + + int *m_jointIndices; + + double *m_targetPositions; + double *m_kps; + + double *m_targetVelocities; + double *m_kds; + + double *m_forces; + + b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) + : m_controlMode(controlMode), m_numControlledDofs(numControlledDofs) + { + } +}; + +struct b3RobotSimulatorGetCameraImageArgs +{ + int m_width; + int m_height; + float *m_viewMatrix; + float *m_projectionMatrix; + float *m_lightDirection; + float *m_lightColor; + float m_lightDistance; + int m_hasShadow; + float m_lightAmbientCoeff; + float m_lightDiffuseCoeff; + float m_lightSpecularCoeff; + int m_renderer; + + b3RobotSimulatorGetCameraImageArgs(int width, int height) + : m_width(width), + m_height(height), + m_viewMatrix(NULL), + m_projectionMatrix(NULL), + m_lightDirection(NULL), + m_lightColor(NULL), + m_lightDistance(-1), + m_hasShadow(-1), + m_lightAmbientCoeff(-1), + m_lightDiffuseCoeff(-1), + m_lightSpecularCoeff(-1), + m_renderer(-1) + { + } +}; + +struct b3RobotSimulatorSetPhysicsEngineParameters +{ + double m_fixedTimeStep; + int m_numSolverIterations; + int m_useSplitImpulse; + double m_splitImpulsePenetrationThreshold; + int m_numSubSteps; + int m_collisionFilterMode; + double m_contactBreakingThreshold; + int m_maxNumCmdPer1ms; + int m_enableFileCaching; + double m_restitutionVelocityThreshold; + double m_erp; + double m_contactERP; + double m_frictionERP; + + b3RobotSimulatorSetPhysicsEngineParameters() + : m_fixedTimeStep(-1), + m_numSolverIterations(-1), + m_useSplitImpulse(-1), + m_splitImpulsePenetrationThreshold(-1), + m_numSubSteps(-1), + m_collisionFilterMode(-1), + m_contactBreakingThreshold(-1), + m_maxNumCmdPer1ms(-1), + m_enableFileCaching(-1), + m_restitutionVelocityThreshold(-1), + m_erp(-1), + m_contactERP(-1), + m_frictionERP(-1) + {} +}; + +struct b3RobotSimulatorChangeDynamicsArgs +{ + double m_mass; + double m_lateralFriction; + double m_spinningFriction; + double m_rollingFriction; + double m_restitution; + double m_linearDamping; + double m_angularDamping; + double m_contactStiffness; + double m_contactDamping; + int m_frictionAnchor; + + b3RobotSimulatorChangeDynamicsArgs() + : m_mass(-1), + m_lateralFriction(-1), + m_spinningFriction(-1), + m_rollingFriction(-1), + m_restitution(-1), + m_linearDamping(-1), + m_angularDamping(-1), + m_contactStiffness(-1), + m_contactDamping(-1), + m_frictionAnchor(-1) + {} +}; + +struct b3RobotSimulatorAddUserDebugLineArgs +{ + double m_colorRGB[3]; + double m_lineWidth; + double m_lifeTime; + int m_parentObjectUniqueId; + int m_parentLinkIndex; + + b3RobotSimulatorAddUserDebugLineArgs() + : + m_lineWidth(1), + m_lifeTime(0), + m_parentObjectUniqueId(-1), + m_parentLinkIndex(-1) + { + m_colorRGB[0] = 1; + m_colorRGB[1] = 1; + m_colorRGB[2] = 1; + } +}; + +enum b3AddUserDebugTextFlags { + DEBUG_TEXT_HAS_ORIENTATION = 1 +}; + +struct b3RobotSimulatorAddUserDebugTextArgs +{ + double m_colorRGB[3]; + double m_size; + double m_lifeTime; + double m_textOrientation[4]; + int m_parentObjectUniqueId; + int m_parentLinkIndex; + int m_flags; + + b3RobotSimulatorAddUserDebugTextArgs() + : m_size(1), + m_lifeTime(0), + m_parentObjectUniqueId(-1), + m_parentLinkIndex(-1), + m_flags(0) + { + m_colorRGB[0] = 1; + m_colorRGB[1] = 1; + m_colorRGB[2] = 1; + + m_textOrientation[0] = 0; + m_textOrientation[1] = 0; + m_textOrientation[2] = 0; + m_textOrientation[3] = 1; + + } +}; + +struct b3RobotSimulatorGetContactPointsArgs +{ + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + + b3RobotSimulatorGetContactPointsArgs() + : m_bodyUniqueIdA(-1), + m_bodyUniqueIdB(-1), + m_linkIndexA(-2), + m_linkIndexB(-2) + {} +}; + +struct b3RobotSimulatorCreateCollisionShapeArgs +{ + int m_shapeType; + double m_radius; + b3Vector3 m_halfExtents; + double m_height; + char* m_fileName; + b3Vector3 m_meshScale; + b3Vector3 m_planeNormal; + int m_flags; + b3RobotSimulatorCreateCollisionShapeArgs() + : m_shapeType(-1), + m_radius(0.5), + m_height(1), + m_fileName(NULL), + m_flags(0) + { + m_halfExtents.m_floats[0] = 1; + m_halfExtents.m_floats[1] = 1; + m_halfExtents.m_floats[2] = 1; + + m_meshScale.m_floats[0] = 1; + m_meshScale.m_floats[1] = 1; + m_meshScale.m_floats[2] = 1; + + m_planeNormal.m_floats[0] = 0; + m_planeNormal.m_floats[1] = 0; + m_planeNormal.m_floats[2] = 1; + } + +}; + +struct b3RobotSimulatorCreateMultiBodyArgs +{ + double m_baseMass; + int m_baseCollisionShapeIndex; + int m_baseVisualShapeIndex; + b3Vector3 m_basePosition; + b3Quaternion m_baseOrientation; + b3Vector3 m_baseInertialFramePosition; + b3Quaternion m_baseInertialFrameOrientation; + + int m_numLinks; + double *m_linkMasses; + int *m_linkCollisionShapeIndices; + int *m_linkVisualShapeIndices; + b3Vector3 *m_linkPositions; + b3Quaternion *m_linkOrientations; + b3Vector3 *m_linkInertialFramePositions; + b3Quaternion *m_linkInertialFrameOrientations; + int *m_linkParentIndices; + int *m_linkJointTypes; + b3Vector3 *m_linkJointAxes; + + int m_useMaximalCoordinates; + + b3RobotSimulatorCreateMultiBodyArgs() + : m_numLinks(0), m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_useMaximalCoordinates(0), + m_linkMasses(NULL), + m_linkCollisionShapeIndices(NULL), + m_linkVisualShapeIndices(NULL), + m_linkPositions(NULL), + m_linkOrientations(NULL), + m_linkInertialFramePositions(NULL), + m_linkInertialFrameOrientations(NULL), + m_linkParentIndices(NULL), + m_linkJointTypes(NULL), + m_linkJointAxes(NULL) + { + m_basePosition.setValue(0,0,0); + m_baseOrientation.setValue(0,0,0,1); + m_baseInertialFramePosition.setValue(0,0,0); + m_baseInertialFrameOrientation.setValue(0,0,0,1); + } +}; + + + +///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 +{ +protected: + + struct b3RobotSimulatorClientAPI_InternalData* m_data; + +public: + + b3RobotSimulatorClientAPI_NoGUI(); + virtual ~b3RobotSimulatorClientAPI_NoGUI(); + + bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + + void disconnect(); + + bool isConnected() const; + + void setTimeOut(double timeOutInSec); + + 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 getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); + + bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; + bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); + + bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; + bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; + + int getNumJoints(int bodyUniqueId) const; + + bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); + + int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + + int changeConstraint(int constraintId, b3JointInfo* jointInfo); + + void removeConstraint(int constraintId); + + bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); + + bool getJointStates(int bodyUniqueId, b3JointStates2& state); + + bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); + + void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); + + bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs, + int *jointIndices, double *targetVelocities, double *targetPositions, + double *forces, double *kps, double *kds); + + void stepSimulation(); + + bool canSubmitCommand() const; + + void setRealTimeSimulation(bool enableRealTimeSimulation); + + void setInternalSimFlags(int flags); + + void setGravity(const b3Vector3& gravityAcceleration); + + void setTimeStep(double timeStepInSeconds); + void setNumSimulationSubSteps(int numSubSteps); + void setNumSolverIterations(int numIterations); + void setContactBreakingThreshold(double threshold); + + 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); + void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos); + + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); + void stopStateLogging(int stateLoggerUniqueId); + + void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter); + void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); + + void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); + + // JFC: added these 24 methods + + void getMouseEvents(b3MouseEventsData* mouseEventsData); + + bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeInverseKinematics, b3LinkState* linkState); + + bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData); + + bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput); + + int getNumBodies() const; + + int getBodyUniqueId(int bodyId) const; + + bool removeBody(int bodyUniqueId); + + bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo); + + bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args); + + int addUserDebugParameter(char *paramName, double rangeMin, double rangeMax, double startValue); + + double readUserDebugParameter(int itemUniqueId); + + bool removeUserDebugItem(int itemUniqueId); + + int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + + int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + + int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + + int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); + + bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args); + + bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args); + + bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags); + + bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags); + + bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags); + + bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags); + + bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable); + + bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo); + + bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo); + + bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo); + + bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData); + + bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData); + + bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax); + + bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax); + + int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); + + int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args); + + int getNumConstraints() const; + + int getConstraintUniqueId(int serialIndex); + + void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin); + + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); + virtual struct GUIHelperInterface* getGuiHelper(); + +}; + +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index 020ef2c64..f24ba6b90 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -193,6 +193,8 @@ if not _OPTIONS["no-enet"] then "RobotSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", + "b3RobotSimulatorClientAPI_NoGUI.cpp", + "b3RobotSimulatorClientAPI_NoGUI.h", "MinitaurSetup.cpp", "MinitaurSetup.h", myfiles @@ -283,6 +285,8 @@ project ("App_VRGloveHandSimulator") "VRGloveSimulatorMain.cpp", "b3RobotSimulatorClientAPI.cpp", "b3RobotSimulatorClientAPI.h", + "b3RobotSimulatorClientAPI_NoGUI.cpp", + "b3RobotSimulatorClientAPI_NoGUI.h", myfiles } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 34f5d9f31..6dea73129 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -32,7 +32,6 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle); #include "PhysicsClientTCP_C_API.h" #endif -#include "SharedMemoryInProcessPhysicsC_API.h" #ifdef __cplusplus extern "C" { diff --git a/examples/TwoJoint/TwoJointMain.cpp b/examples/TwoJoint/TwoJointMain.cpp index 6c25fe5be..64e325bf5 100644 --- a/examples/TwoJoint/TwoJointMain.cpp +++ b/examples/TwoJoint/TwoJointMain.cpp @@ -9,6 +9,7 @@ #include "SharedMemory/PhysicsClientC_API.h" #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Quaternion.h" +#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h" extern const int CONTROL_RATE; const int CONTROL_RATE = 500;