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/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index e9f1194fe..c7cac6709 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -2085,13 +2085,14 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode) b3CreateOrtho(-shadowMapWorldSize,shadowMapWorldSize,-shadowMapWorldSize,shadowMapWorldSize,1,300,depthProjectionMatrix);//-14,14,-14,14,1,200, depthProjectionMatrix); float depthViewMatrix[4][4]; b3Vector3 center = b3MakeVector3(0,0,0); + m_data->m_activeCamera->getCameraTargetPosition(center); //float upf[3]; //m_data->m_activeCamera->getCameraUpVector(upf); b3Vector3 up, lightFwd; b3Vector3 lightDir = m_data->m_lightPos.normalized(); b3PlaneSpace1(lightDir,up,lightFwd); // b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]); - b3CreateLookAt(m_data->m_lightPos,center,up,&depthViewMatrix[0][0]); + b3CreateLookAt(m_data->m_lightPos+center,center,up,&depthViewMatrix[0][0]); //b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2); GLfloat depthModelMatrix[4][4]; 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; diff --git a/examples/pybullet/gym/pybullet_data/bicycle/LICENSE.txt b/examples/pybullet/gym/pybullet_data/bicycle/LICENSE.txt new file mode 100644 index 000000000..e6e493d49 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/bicycle/LICENSE.txt @@ -0,0 +1,362 @@ +Functional Bicycle (http://www.thingiverse.com/thing:309158) by thomasgivan is licensed under the Creative Commons - Attribution - Share Alike license. +http://creativecommons.org/licenses/by-sa/3.0/ + +Creative Commons Legal Code + +Attribution-ShareAlike 3.0 Unported + + CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE + LEGAL SERVICES. DISTRIBUTION OF THIS LICENSE DOES NOT CREATE AN + ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS + INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES + REGARDING THE INFORMATION PROVIDED, AND DISCLAIMS LIABILITY FOR + DAMAGES RESULTING FROM ITS USE. + +License + +THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE +COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY +COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS +AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + +BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE +TO BE BOUND BY THE TERMS OF THIS LICENSE. TO THE EXTENT THIS LICENSE MAY +BE CONSIDERED TO BE A CONTRACT, THE LICENSOR GRANTS YOU THE RIGHTS +CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND +CONDITIONS. + +1. Definitions + + a. "Adaptation" means a work based upon the Work, or upon the Work and + other pre-existing works, such as a translation, adaptation, + derivative work, arrangement of music or other alterations of a + literary or artistic work, or phonogram or performance and includes + cinematographic adaptations or any other form in which the Work may be + recast, transformed, or adapted including in any form recognizably + derived from the original, except that a work that constitutes a + Collection will not be considered an Adaptation for the purpose of + this License. For the avoidance of doubt, where the Work is a musical + work, performance or phonogram, the synchronization of the Work in + timed-relation with a moving image ("synching") will be considered an + Adaptation for the purpose of this License. + b. "Collection" means a collection of literary or artistic works, such as + encyclopedias and anthologies, or performances, phonograms or + broadcasts, or other works or subject matter other than works listed + in Section 1(f) below, which, by reason of the selection and + arrangement of their contents, constitute intellectual creations, in + which the Work is included in its entirety in unmodified form along + with one or more other contributions, each constituting separate and + independent works in themselves, which together are assembled into a + collective whole. A work that constitutes a Collection will not be + considered an Adaptation (as defined below) for the purposes of this + License. + c. "Creative Commons Compatible License" means a license that is listed + at https://creativecommons.org/compatiblelicenses that has been + approved by Creative Commons as being essentially equivalent to this + License, including, at a minimum, because that license: (i) contains + terms that have the same purpose, meaning and effect as the License + Elements of this License; and, (ii) explicitly permits the relicensing + of adaptations of works made available under that license under this + License or a Creative Commons jurisdiction license with the same + License Elements as this License. + d. "Distribute" means to make available to the public the original and + copies of the Work or Adaptation, as appropriate, through sale or + other transfer of ownership. + e. "License Elements" means the following high-level license attributes + as selected by Licensor and indicated in the title of this License: + Attribution, ShareAlike. + f. "Licensor" means the individual, individuals, entity or entities that + offer(s) the Work under the terms of this License. + g. "Original Author" means, in the case of a literary or artistic work, + the individual, individuals, entity or entities who created the Work + or if no individual or entity can be identified, the publisher; and in + addition (i) in the case of a performance the actors, singers, + musicians, dancers, and other persons who act, sing, deliver, declaim, + play in, interpret or otherwise perform literary or artistic works or + expressions of folklore; (ii) in the case of a phonogram the producer + being the person or legal entity who first fixes the sounds of a + performance or other sounds; and, (iii) in the case of broadcasts, the + organization that transmits the broadcast. + h. "Work" means the literary and/or artistic work offered under the terms + of this License including without limitation any production in the + literary, scientific and artistic domain, whatever may be the mode or + form of its expression including digital form, such as a book, + pamphlet and other writing; a lecture, address, sermon or other work + of the same nature; a dramatic or dramatico-musical work; a + choreographic work or entertainment in dumb show; a musical + composition with or without words; a cinematographic work to which are + assimilated works expressed by a process analogous to cinematography; + a work of drawing, painting, architecture, sculpture, engraving or + lithography; a photographic work to which are assimilated works + expressed by a process analogous to photography; a work of applied + art; an illustration, map, plan, sketch or three-dimensional work + relative to geography, topography, architecture or science; a + performance; a broadcast; a phonogram; a compilation of data to the + extent it is protected as a copyrightable work; or a work performed by + a variety or circus performer to the extent it is not otherwise + considered a literary or artistic work. + i. "You" means an individual or entity exercising rights under this + License who has not previously violated the terms of this License with + respect to the Work, or who has received express permission from the + Licensor to exercise rights under this License despite a previous + violation. + j. "Publicly Perform" means to perform public recitations of the Work and + to communicate to the public those public recitations, by any means or + process, including by wire or wireless means or public digital + performances; to make available to the public Works in such a way that + members of the public may access these Works from a place and at a + place individually chosen by them; to perform the Work to the public + by any means or process and the communication to the public of the + performances of the Work, including by public digital performance; to + broadcast and rebroadcast the Work by any means including signs, + sounds or images. + k. "Reproduce" means to make copies of the Work by any means including + without limitation by sound or visual recordings and the right of + fixation and reproducing fixations of the Work, including storage of a + protected performance or phonogram in digital form or other electronic + medium. + +2. Fair Dealing Rights. Nothing in this License is intended to reduce, +limit, or restrict any uses free from copyright or rights arising from +limitations or exceptions that are provided for in connection with the +copyright protection under copyright law or other applicable laws. + +3. License Grant. Subject to the terms and conditions of this License, +Licensor hereby grants You a worldwide, royalty-free, non-exclusive, +perpetual (for the duration of the applicable copyright) license to +exercise the rights in the Work as stated below: + + a. to Reproduce the Work, to incorporate the Work into one or more + Collections, and to Reproduce the Work as incorporated in the + Collections; + b. to create and Reproduce Adaptations provided that any such Adaptation, + including any translation in any medium, takes reasonable steps to + clearly label, demarcate or otherwise identify that changes were made + to the original Work. For example, a translation could be marked "The + original work was translated from English to Spanish," or a + modification could indicate "The original work has been modified."; + c. to Distribute and Publicly Perform the Work including as incorporated + in Collections; and, + d. to Distribute and Publicly Perform Adaptations. + e. For the avoidance of doubt: + + i. Non-waivable Compulsory License Schemes. In those jurisdictions in + which the right to collect royalties through any statutory or + compulsory licensing scheme cannot be waived, the Licensor + reserves the exclusive right to collect such royalties for any + exercise by You of the rights granted under this License; + ii. Waivable Compulsory License Schemes. In those jurisdictions in + which the right to collect royalties through any statutory or + compulsory licensing scheme can be waived, the Licensor waives the + exclusive right to collect such royalties for any exercise by You + of the rights granted under this License; and, + iii. Voluntary License Schemes. The Licensor waives the right to + collect royalties, whether individually or, in the event that the + Licensor is a member of a collecting society that administers + voluntary licensing schemes, via that society, from any exercise + by You of the rights granted under this License. + +The above rights may be exercised in all media and formats whether now +known or hereafter devised. The above rights include the right to make +such modifications as are technically necessary to exercise the rights in +other media and formats. Subject to Section 8(f), all rights not expressly +granted by Licensor are hereby reserved. + +4. Restrictions. The license granted in Section 3 above is expressly made +subject to and limited by the following restrictions: + + a. You may Distribute or Publicly Perform the Work only under the terms + of this License. You must include a copy of, or the Uniform Resource + Identifier (URI) for, this License with every copy of the Work You + Distribute or Publicly Perform. You may not offer or impose any terms + on the Work that restrict the terms of this License or the ability of + the recipient of the Work to exercise the rights granted to that + recipient under the terms of the License. You may not sublicense the + Work. You must keep intact all notices that refer to this License and + to the disclaimer of warranties with every copy of the Work You + Distribute or Publicly Perform. When You Distribute or Publicly + Perform the Work, You may not impose any effective technological + measures on the Work that restrict the ability of a recipient of the + Work from You to exercise the rights granted to that recipient under + the terms of the License. This Section 4(a) applies to the Work as + incorporated in a Collection, but this does not require the Collection + apart from the Work itself to be made subject to the terms of this + License. If You create a Collection, upon notice from any Licensor You + must, to the extent practicable, remove from the Collection any credit + as required by Section 4(c), as requested. If You create an + Adaptation, upon notice from any Licensor You must, to the extent + practicable, remove from the Adaptation any credit as required by + Section 4(c), as requested. + b. You may Distribute or Publicly Perform an Adaptation only under the + terms of: (i) this License; (ii) a later version of this License with + the same License Elements as this License; (iii) a Creative Commons + jurisdiction license (either this or a later license version) that + contains the same License Elements as this License (e.g., + Attribution-ShareAlike 3.0 US)); (iv) a Creative Commons Compatible + License. If you license the Adaptation under one of the licenses + mentioned in (iv), you must comply with the terms of that license. If + you license the Adaptation under the terms of any of the licenses + mentioned in (i), (ii) or (iii) (the "Applicable License"), you must + comply with the terms of the Applicable License generally and the + following provisions: (I) You must include a copy of, or the URI for, + the Applicable License with every copy of each Adaptation You + Distribute or Publicly Perform; (II) You may not offer or impose any + terms on the Adaptation that restrict the terms of the Applicable + License or the ability of the recipient of the Adaptation to exercise + the rights granted to that recipient under the terms of the Applicable + License; (III) You must keep intact all notices that refer to the + Applicable License and to the disclaimer of warranties with every copy + of the Work as included in the Adaptation You Distribute or Publicly + Perform; (IV) when You Distribute or Publicly Perform the Adaptation, + You may not impose any effective technological measures on the + Adaptation that restrict the ability of a recipient of the Adaptation + from You to exercise the rights granted to that recipient under the + terms of the Applicable License. This Section 4(b) applies to the + Adaptation as incorporated in a Collection, but this does not require + the Collection apart from the Adaptation itself to be made subject to + the terms of the Applicable License. + c. If You Distribute, or Publicly Perform the Work or any Adaptations or + Collections, You must, unless a request has been made pursuant to + Section 4(a), keep intact all copyright notices for the Work and + provide, reasonable to the medium or means You are utilizing: (i) the + name of the Original Author (or pseudonym, if applicable) if supplied, + and/or if the Original Author and/or Licensor designate another party + or parties (e.g., a sponsor institute, publishing entity, journal) for + attribution ("Attribution Parties") in Licensor's copyright notice, + terms of service or by other reasonable means, the name of such party + or parties; (ii) the title of the Work if supplied; (iii) to the + extent reasonably practicable, the URI, if any, that Licensor + specifies to be associated with the Work, unless such URI does not + refer to the copyright notice or licensing information for the Work; + and (iv) , consistent with Ssection 3(b), in the case of an + Adaptation, a credit identifying the use of the Work in the Adaptation + (e.g., "French translation of the Work by Original Author," or + "Screenplay based on original Work by Original Author"). The credit + required by this Section 4(c) may be implemented in any reasonable + manner; provided, however, that in the case of a Adaptation or + Collection, at a minimum such credit will appear, if a credit for all + contributing authors of the Adaptation or Collection appears, then as + part of these credits and in a manner at least as prominent as the + credits for the other contributing authors. For the avoidance of + doubt, You may only use the credit required by this Section for the + purpose of attribution in the manner set out above and, by exercising + Your rights under this License, You may not implicitly or explicitly + assert or imply any connection with, sponsorship or endorsement by the + Original Author, Licensor and/or Attribution Parties, as appropriate, + of You or Your use of the Work, without the separate, express prior + written permission of the Original Author, Licensor and/or Attribution + Parties. + d. Except as otherwise agreed in writing by the Licensor or as may be + otherwise permitted by applicable law, if You Reproduce, Distribute or + Publicly Perform the Work either by itself or as part of any + Adaptations or Collections, You must not distort, mutilate, modify or + take other derogatory action in relation to the Work which would be + prejudicial to the Original Author's honor or reputation. Licensor + agrees that in those jurisdictions (e.g. Japan), in which any exercise + of the right granted in Section 3(b) of this License (the right to + make Adaptations) would be deemed to be a distortion, mutilation, + modification or other derogatory action prejudicial to the Original + Author's honor and reputation, the Licensor will waive or not assert, + as appropriate, this Section, to the fullest extent permitted by the + applicable national law, to enable You to reasonably exercise Your + right under Section 3(b) of this License (right to make Adaptations) + but not otherwise. + +5. Representations, Warranties and Disclaimer + +UNLESS OTHERWISE MUTUALLY AGREED TO BY THE PARTIES IN WRITING, LICENSOR +OFFERS THE WORK AS-IS AND MAKES NO REPRESENTATIONS OR WARRANTIES OF ANY +KIND CONCERNING THE WORK, EXPRESS, IMPLIED, STATUTORY OR OTHERWISE, +INCLUDING, WITHOUT LIMITATION, WARRANTIES OF TITLE, MERCHANTIBILITY, +FITNESS FOR A PARTICULAR PURPOSE, NONINFRINGEMENT, OR THE ABSENCE OF +LATENT OR OTHER DEFECTS, ACCURACY, OR THE PRESENCE OF ABSENCE OF ERRORS, +WHETHER OR NOT DISCOVERABLE. SOME JURISDICTIONS DO NOT ALLOW THE EXCLUSION +OF IMPLIED WARRANTIES, SO SUCH EXCLUSION MAY NOT APPLY TO YOU. + +6. Limitation on Liability. EXCEPT TO THE EXTENT REQUIRED BY APPLICABLE +LAW, IN NO EVENT WILL LICENSOR BE LIABLE TO YOU ON ANY LEGAL THEORY FOR +ANY SPECIAL, INCIDENTAL, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES +ARISING OUT OF THIS LICENSE OR THE USE OF THE WORK, EVEN IF LICENSOR HAS +BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. + +7. Termination + + a. This License and the rights granted hereunder will terminate + automatically upon any breach by You of the terms of this License. + Individuals or entities who have received Adaptations or Collections + from You under this License, however, will not have their licenses + terminated provided such individuals or entities remain in full + compliance with those licenses. Sections 1, 2, 5, 6, 7, and 8 will + survive any termination of this License. + b. Subject to the above terms and conditions, the license granted here is + perpetual (for the duration of the applicable copyright in the Work). + Notwithstanding the above, Licensor reserves the right to release the + Work under different license terms or to stop distributing the Work at + any time; provided, however that any such election will not serve to + withdraw this License (or any other license that has been, or is + required to be, granted under the terms of this License), and this + License will continue in full force and effect unless terminated as + stated above. + +8. Miscellaneous + + a. Each time You Distribute or Publicly Perform the Work or a Collection, + the Licensor offers to the recipient a license to the Work on the same + terms and conditions as the license granted to You under this License. + b. Each time You Distribute or Publicly Perform an Adaptation, Licensor + offers to the recipient a license to the original Work on the same + terms and conditions as the license granted to You under this License. + c. If any provision of this License is invalid or unenforceable under + applicable law, it shall not affect the validity or enforceability of + the remainder of the terms of this License, and without further action + by the parties to this agreement, such provision shall be reformed to + the minimum extent necessary to make such provision valid and + enforceable. + d. No term or provision of this License shall be deemed waived and no + breach consented to unless such waiver or consent shall be in writing + and signed by the party to be charged with such waiver or consent. + e. This License constitutes the entire agreement between the parties with + respect to the Work licensed here. There are no understandings, + agreements or representations with respect to the Work not specified + here. Licensor shall not be bound by any additional provisions that + may appear in any communication from You. This License may not be + modified without the mutual written agreement of the Licensor and You. + f. The rights granted under, and the subject matter referenced, in this + License were drafted utilizing the terminology of the Berne Convention + for the Protection of Literary and Artistic Works (as amended on + September 28, 1979), the Rome Convention of 1961, the WIPO Copyright + Treaty of 1996, the WIPO Performances and Phonograms Treaty of 1996 + and the Universal Copyright Convention (as revised on July 24, 1971). + These rights and subject matter take effect in the relevant + jurisdiction in which the License terms are sought to be enforced + according to the corresponding provisions of the implementation of + those treaty provisions in the applicable national law. If the + standard suite of rights granted under applicable copyright law + includes additional rights not granted under this License, such + additional rights are deemed to be included in the License; this + License is not intended to restrict the license of any rights under + applicable law. + + +Creative Commons Notice + + Creative Commons is not a party to this License, and makes no warranty + whatsoever in connection with the Work. Creative Commons will not be + liable to You or any party on any legal theory for any damages + whatsoever, including without limitation any general, special, + incidental or consequential damages arising in connection to this + license. Notwithstanding the foregoing two (2) sentences, if Creative + Commons has expressly identified itself as the Licensor hereunder, it + shall have all rights and obligations of Licensor. + + Except for the limited purpose of indicating to the public that the + Work is licensed under the CCPL, Creative Commons does not authorize + the use by either party of the trademark "Creative Commons" or any + related trademark or logo of Creative Commons without the prior + written consent of Creative Commons. Any permitted use will be in + compliance with Creative Commons' then-current trademark usage + guidelines, as may be published on its website or otherwise made + available upon request from time to time. For the avoidance of doubt, + this trademark restriction does not form part of the License. + + Creative Commons may be contacted at https://creativecommons.org/. \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/bicycle/bike.urdf b/examples/pybullet/gym/pybullet_data/bicycle/bike.urdf new file mode 100644 index 000000000..d0e1264f6 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/bicycle/bike.urdf @@ -0,0 +1,143 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/all_pedal_parts.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/all_pedal_parts.stl new file mode 100644 index 000000000..dc8057982 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/all_pedal_parts.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/bike_rack.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/bike_rack.stl new file mode 100644 index 000000000..60b530293 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/bike_rack.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled.stl new file mode 100644 index 000000000..1e507b880 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled_left.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled_left.stl new file mode 100644 index 000000000..2b31a45e7 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled_left.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled_right.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled_right.stl new file mode 100644 index 000000000..1e507b880 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/frame_scaled_right.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/handlebar_scaled.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/handlebar_scaled.stl new file mode 100644 index 000000000..7cf562b57 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/handlebar_scaled.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/wheel_axels.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/wheel_axels.stl new file mode 100644 index 000000000..207cc0940 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/wheel_axels.stl differ diff --git a/examples/pybullet/gym/pybullet_data/bicycle/files/wheel_scaled.stl b/examples/pybullet/gym/pybullet_data/bicycle/files/wheel_scaled.stl new file mode 100644 index 000000000..1474b5486 Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/bicycle/files/wheel_scaled.stl differ diff --git a/examples/pybullet/gym/pybullet_data/plane100.obj b/examples/pybullet/gym/pybullet_data/plane100.obj new file mode 100644 index 000000000..3a74f590b --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/plane100.obj @@ -0,0 +1,22 @@ +# Blender v2.66 (sub 1) OBJ File: '' +# www.blender.org +mtllib plane.mtl +o Plane +v 100.000000 -100.000000 0.000000 +v 100.000000 100.000000 0.000000 +v -100.000000 100.000000 0.000000 +v -100.000000 -100.000000 0.000000 + +vt 100.000000 0.000000 +vt 100.000000 100.000000 +vt 0.000000 100.000000 +vt 0.000000 0.000000 + + + +usemtl Material +s off +f 1/1 2/2 3/3 +f 1/1 3/3 4/4 + + diff --git a/examples/pybullet/gym/pybullet_data/plane100.urdf b/examples/pybullet/gym/pybullet_data/plane100.urdf new file mode 100644 index 000000000..f83006900 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/plane100.urdf @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py index e28a9e962..277e52ba2 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py @@ -64,10 +64,10 @@ class CartPoleBulletEnv(gym.Env): # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state - + dv = 0.1 deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] - + p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) done = x < -self.x_threshold \ @@ -99,3 +99,8 @@ class CartPoleBulletEnv(gym.Env): def _render(self, mode='human', close=False): return + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py index 4e7d7e70e..8ed171be0 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py @@ -56,8 +56,8 @@ class KukaCamGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -74,15 +74,15 @@ class KukaCamGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.5 +0.2*random.random() ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -98,7 +98,7 @@ class KukaCamGymEnv(gym.Env): return [seed] def getExtendedObservation(self): - + #camEyePos = [0.03,0.236,0.54] #distance = 1.06 #pitch=-56 @@ -118,13 +118,13 @@ class KukaCamGymEnv(gym.Env): viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0] #projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] - + img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) rgb=img_arr[2] np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.01 @@ -142,7 +142,7 @@ class KukaCamGymEnv(gym.Env): realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -158,11 +158,11 @@ class KukaCamGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() reward = self._reward() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -190,18 +190,18 @@ class KukaCamGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -212,7 +212,7 @@ class KukaCamGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -227,18 +227,18 @@ class KukaCamGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) - reward = -1000 + reward = -1000 numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -254,3 +254,7 @@ class KukaCamGymEnv(gym.Env): #print(reward) return reward + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index b9ef16017..6fa57e76a 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -44,7 +44,7 @@ class KukaGymEnv(gym.Env): self._maxSteps = maxSteps self.terminated = 0 self._cam_dist = 1.3 - self._cam_yaw = 180 + self._cam_yaw = 180 self._cam_pitch = -40 self._p = p @@ -61,8 +61,8 @@ class KukaGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([largeValObservation] * observationDim) + + observation_high = np.array([largeValObservation] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -80,15 +80,15 @@ class KukaGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.55 +0.12*random.random() ypos = 0 +0.2*random.random() ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -115,7 +115,7 @@ class KukaGymEnv(gym.Env): dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]] dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]] dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]] - + gripperEul = p.getEulerFromQuaternion(gripperOrn) #print("gripperEul") #print(gripperEul) @@ -126,17 +126,17 @@ class KukaGymEnv(gym.Env): #print(projectedBlockPos2D) #print("blockEulerInGripper") #print(blockEulerInGripper) - + #we return the relative x,y position and euler angle of block in gripper space blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]] - + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) - + self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.005 @@ -154,7 +154,7 @@ class KukaGymEnv(gym.Env): f = 0.3 realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -168,7 +168,7 @@ class KukaGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]]) actionCost = np.linalg.norm(npaction)*10. @@ -177,9 +177,9 @@ class KukaGymEnv(gym.Env): reward = self._reward()-actionCost #print("reward") #print(reward) - + #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode="rgb_array", close=False): @@ -208,18 +208,18 @@ class KukaGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>self._maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("terminating, closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -230,7 +230,7 @@ class KukaGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -245,19 +245,19 @@ class KukaGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) reward = -1000 - + numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -276,10 +276,7 @@ class KukaGymEnv(gym.Env): #print(reward) return reward - def reset(self): - """Resets the state of the environment and returns an initial observation. - - Returns: observation (object): the initial observation of the - space. - """ - return self._reset() \ No newline at end of file + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py index 44b41dd7d..b5c3375b4 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py @@ -385,3 +385,8 @@ class MinitaurBulletDuckEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py index 38ec631a5..158128067 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -376,3 +376,8 @@ class MinitaurBulletEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index 7537d54c1..bdde5d963 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -50,8 +50,8 @@ class RacecarGymEnv(gym.Env): #self.reset() observationDim = 2 #len(self.getExtendedObservation()) #print("observationDim") - #print(observationDim) - # observation_high = np.array([np.finfo(np.float32).max] * observationDim) + #print(observationDim) + # observation_high = np.array([np.finfo(np.float32).max] * observationDim) observation_high = np.ones(observationDim) * 1000 #np.inf if (isDiscrete): self.action_space = spaces.Discrete(9) @@ -59,7 +59,7 @@ class RacecarGymEnv(gym.Env): action_dim = 2 self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) - self.action_space = spaces.Box(-action_high, action_high) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -74,14 +74,14 @@ class RacecarGymEnv(gym.Env): # pos,orn = self._p.getBasePositionAndOrientation(i) # newpos = [pos[0],pos[1],pos[2]-0.1] # self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -101,18 +101,18 @@ class RacecarGymEnv(gym.Env): def getExtendedObservation(self): self._observation = [] #self._racecar.getObservation() carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) - + self._observation.extend([ballPosInCar[0],ballPosInCar[1]]) return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + if (self._isDiscrete): fwd = [-1,-1,-1,0,0,0,1,1,1] steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] @@ -128,14 +128,14 @@ class RacecarGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -159,13 +159,13 @@ class RacecarGymEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array - + def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -174,3 +174,8 @@ class RacecarGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index d9546de8c..277453e14 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -40,7 +40,7 @@ class RacecarZEDGymEnv(gym.Env): self._renders = renders self._width = 100 self._height = 10 - + self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient( @@ -53,8 +53,8 @@ class RacecarZEDGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (isDiscrete): self.action_space = spaces.Discrete(9) else: @@ -77,14 +77,14 @@ class RacecarZEDGymEnv(gym.Env): pos,orn = self._p.getBasePositionAndOrientation(i) newpos = [pos[0],pos[1],pos[2]+0.1] self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2red.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -104,13 +104,13 @@ class RacecarZEDGymEnv(gym.Env): def getExtendedObservation(self): carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) carmat = self._p.getMatrixFromQuaternion(carorn) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) dist0 = 0.3 dist1 = 1. eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3] - targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] + targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] up = [carmat[2],carmat[5],carmat[8]] viewMat = self._p.computeViewMatrix(eyePos,targetPos,up) #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) @@ -122,12 +122,12 @@ class RacecarZEDGymEnv(gym.Env): np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + if (self._isDiscrete): fwd = [-1,-1,-1,0,0,0,1,1,1] steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] @@ -143,14 +143,14 @@ class RacecarZEDGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -177,10 +177,10 @@ class RacecarZEDGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -189,3 +189,8 @@ class RacecarZEDGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py index c632e7f31..53b19b7a8 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py @@ -46,8 +46,8 @@ class SimpleHumanoidGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -57,14 +57,14 @@ class SimpleHumanoidGymEnv(gym.Env): #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf")) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + p.setGravity(0,0,-10) self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -82,7 +82,7 @@ class SimpleHumanoidGymEnv(gym.Env): def getExtendedObservation(self): self._observation = self._humanoid.getObservation() return self._observation - + def _step(self, action): self._humanoid.applyAction(action) for i in range(self._actionRepeat): @@ -96,7 +96,7 @@ class SimpleHumanoidGymEnv(gym.Env): reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -104,8 +104,13 @@ class SimpleHumanoidGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): reward=self._humanoid.distance print(reward) return reward + + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 5d49c22c3..ceafb6404 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -43,7 +43,7 @@ class MJCFBaseBulletEnv(gym.Env): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False - + self.physicsClientId = 0 else: self.ownsPhysicsClient = True @@ -75,12 +75,12 @@ class MJCFBaseBulletEnv(gym.Env): self.isRender = True if mode != "rgb_array": return np.array([]) - + base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz - + view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -100,6 +100,7 @@ class MJCFBaseBulletEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array + def _close(self): if (self.ownsPhysicsClient): if (self.physicsClientId>=0): @@ -109,6 +110,17 @@ class MJCFBaseBulletEnv(gym.Env): def HUD(self, state, a, done): pass + # backwards compatibility for gym >= v0.9.x + # for extension of this class. + def step(self, *args, **kwargs): + return self._step(*args, **kwargs) + + close = _close + render = _render + reset = _reset + seed = _seed + + class Camera: def __init__(self): pass diff --git a/examples/pybullet/gym/pybullet_envs/examples/testBike.py b/examples/pybullet/gym/pybullet_envs/examples/testBike.py new file mode 100644 index 000000000..3d6e89c91 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/testBike.py @@ -0,0 +1,137 @@ +import pybullet as p +import math +import time +import pybullet_data + +p.connect(p.GUI) +#p.loadURDF("wheel.urdf",[0,0,3]) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +plane=p.loadURDF("plane100.urdf",[0,0,0]) +timestep = 1./240. + +bike=-1 +for i in range (1): + + bike=p.loadURDF("bicycle/bike.urdf",[0,0+3*i,1.5], [0,0,0,1], useFixedBase=False) + p.setJointMotorControl2(bike,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0.05) + #p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000) + p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0) + p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20) + + p.changeDynamics(plane,-1, mass=20,lateralFriction=1, linearDamping=0, angularDamping=0) + p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0) + p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0) + #p.resetJointState(bike,1,0,100) + #p.resetJointState(bike,2,0,100) + #p.resetBaseVelocity(bike,[0,0,0],[0,0,0]) +#p.setPhysicsEngineParameter(numSubSteps=0) +#bike=p.loadURDF("frame.urdf",useFixedBase=True) +#bike = p.loadURDF("handlebar.urdf", useFixedBase=True) +#p.loadURDF("handlebar.urdf",[0,2,1]) +#coord = p.loadURDF("handlebar.urdf", [0.7700000000000005, 0, 0.22000000000000006],useFixedBase=True)# p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True) +#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True) +p.setGravity(0,0,-10) +p.setRealTimeSimulation(0) +#coordPos = [0,0,0] +#coordOrnEuler = [0,0,0] + +#coordPos= [0.7000000000000004, 0, 0.22000000000000006] +#coordOrnEuler= [0, -0.2617993877991496, 0] + +coordPos= [0.07, 0, -0.6900000000000004] +coordOrnEuler= [0, 0, 0] + +coordPos2= [0, 0, 0 ] +coordOrnEuler2= [0, 0, 0] + +invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler)) +mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2)) +eul = p.getEulerFromQuaternion(mOrn) +print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2]) + + +shift=0 +gui = 1 + + +frame=0 +states=[] +states.append(p.saveState()) +#observations=[] +#observations.append(obs) + +running = True +reverting = False +p.getCameraImage(320,200)#,renderer=p.ER_BULLET_HARDWARE_OPENGL ) + +while (1): + + updateCam = 0 + keys = p.getKeyboardEvents() + for k,v in keys.items(): + if (reverting or (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED))): + reverting=True + stateIndex = len(states)-1 + #print("prestateIndex=",stateIndex) + time.sleep(timestep) + updateCam=1 + if (stateIndex>0): + stateIndex-=1 + states=states[:stateIndex+1] + #observations=observations[:stateIndex+1] + + + #print("states=",states) + #print("stateIndex =",stateIndex ) + p.restoreState(states[stateIndex]) + #obs=observations[stateIndex] + + + #obs, r, done, _ = env.step(a) + if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)): + reverting = False + + if (k == ord('1') and (v&p.KEY_WAS_TRIGGERED)): + gui = not gui + + + + if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)): + running=False + + if (running or (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED))): + running = True + + if (running): + + p.stepSimulation() + + updateCam=1 + time.sleep(timestep) + pts = p.getContactPoints() + #print("numPoints=",len(pts)) + #for point in pts: + # print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9]) + + states.append(p.saveState()) + #observations.append(obs) + stateIndex = len(states) + #print("stateIndex =",stateIndex ) + frame += 1 + if (updateCam): + distance=5 + yaw = 0 + humanPos, humanOrn = p.getBasePositionAndOrientation(bike) + humanBaseVel = p.getBaseVelocity(bike) + #print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel) + if (gui): + + camInfo = p.getDebugVisualizerCamera() + curTargetPos = camInfo[11] + distance=camInfo[10] + yaw = camInfo[8] + pitch=camInfo[9] + targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]] + p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos); + + diff --git a/examples/pybullet/unity3d/autogen/NativeMethods.cs b/examples/pybullet/unity3d/autogen/NativeMethods.cs index ec5e1543f..532eef557 100644 --- a/examples/pybullet/unity3d/autogen/NativeMethods.cs +++ b/examples/pybullet/unity3d/autogen/NativeMethods.cs @@ -21,8 +21,11 @@ public partial class NativeConstants { /// SHARED_MEMORY_KEY -> 12347 public const int SHARED_MEMORY_KEY = 12347; - /// SHARED_MEMORY_MAGIC_NUMBER -> 201709260 - public const int SHARED_MEMORY_MAGIC_NUMBER = 201709260; + /// SHARED_MEMORY_MAGIC_NUMBER -> 201801170 + public const int SHARED_MEMORY_MAGIC_NUMBER = 201801170; + + /// MAX_VR_ANALOG_AXIS -> 5 + public const int MAX_VR_ANALOG_AXIS = 5; /// MAX_VR_BUTTONS -> 64 public const int MAX_VR_BUTTONS = 64; @@ -42,6 +45,9 @@ public partial class NativeConstants { /// MAX_MOUSE_EVENTS -> 256 public const int MAX_MOUSE_EVENTS = 256; + /// MAX_SDF_BODIES -> 512 + public const int MAX_SDF_BODIES = 512; + /// VISUAL_SHAPE_MAX_PATH_LEN -> 1024 public const int VISUAL_SHAPE_MAX_PATH_LEN = 1024; @@ -92,7 +98,7 @@ public enum EnumSharedMemoryClientCommand { CMD_LOAD_MJCF, - CMD_LOAD_BUNNY, + CMD_LOAD_SOFT_BODY, CMD_SEND_BULLET_DATA_STREAM, @@ -138,6 +144,8 @@ public enum EnumSharedMemoryClientCommand { CMD_CALCULATE_JACOBIAN, + CMD_CALCULATE_MASS_MATRIX, + CMD_USER_CONSTRAINT, CMD_REQUEST_CONTACT_POINT_INFORMATION, @@ -196,6 +204,14 @@ public enum EnumSharedMemoryClientCommand { CMD_CUSTOM_COMMAND, + CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS, + + CMD_SAVE_STATE, + + CMD_RESTORE_STATE, + + CMD_REQUEST_COLLISION_SHAPE_INFO, + CMD_MAX_CLIENT_COMMANDS, } @@ -276,6 +292,10 @@ public enum EnumSharedMemoryServerStatus { CMD_CALCULATED_JACOBIAN_FAILED, + CMD_CALCULATED_MASS_MATRIX_COMPLETED, + + CMD_CALCULATED_MASS_MATRIX_FAILED, + CMD_CONTACT_POINT_INFORMATION_COMPLETED, CMD_CONTACT_POINT_INFORMATION_FAILED, @@ -314,6 +334,8 @@ public enum EnumSharedMemoryServerStatus { CMD_USER_CONSTRAINT_INFO_COMPLETED, + CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED, + CMD_REMOVE_USER_CONSTRAINT_COMPLETED, CMD_CHANGE_USER_CONSTRAINT_COMPLETED, @@ -378,6 +400,24 @@ public enum EnumSharedMemoryServerStatus { CMD_CUSTOM_COMMAND_FAILED, + CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED, + + CMD_SAVE_STATE_FAILED, + + CMD_SAVE_STATE_COMPLETED, + + CMD_RESTORE_STATE_FAILED, + + CMD_RESTORE_STATE_COMPLETED, + + CMD_COLLISION_SHAPE_INFO_COMPLETED, + + CMD_COLLISION_SHAPE_INFO_FAILED, + + CMD_LOAD_SOFT_BODY_FAILED, + + CMD_LOAD_SOFT_BODY_COMPLETED, + CMD_MAX_SERVER_COMMANDS, } @@ -423,15 +463,15 @@ public enum b3JointInfoFlags { eJointChangeChildFrameOrientation = 4, } -[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential, CharSet=System.Runtime.InteropServices.CharSet.Ansi)] public struct b3JointInfo { - + /// char[1024] - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst=1024)] public string m_linkName; - + /// char[1024] - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst=1024)] public string m_jointName; /// int @@ -478,6 +518,9 @@ public struct b3JointInfo { /// double[3] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] public double[] m_jointAxis; + + /// int + public int m_parentIndex; } [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] @@ -529,15 +572,15 @@ public struct b3UserConstraint { public double m_erp; } -[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential, CharSet=System.Runtime.InteropServices.CharSet.Ansi)] public struct b3BodyInfo { - + /// char[1024] - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst=1024)] public string m_baseName; - + /// char[1024] - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst=1024)] public string m_bodyName; } @@ -549,10 +592,29 @@ public struct b3DynamicsInfo { /// double[3] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] - public double[] m_localInertialPosition; + public double[] m_localInertialDiagonal; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_localInertialFrame; /// double public double m_lateralFrictionCoeff; + + /// double + public double m_rollingFrictionCoeff; + + /// double + public double m_spinningFrictionCoeff; + + /// double + public double m_restitution; + + /// double + public double m_contactStiffness; + + /// double + public double m_contactDamping; } public enum SensorType { @@ -624,7 +686,8 @@ public struct b3CameraImageData { public int m_pixelHeight; /// char* - public System.IntPtr m_rgbColorData; + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + public string m_rgbColorData; /// float* public System.IntPtr m_depthValues; @@ -680,6 +743,17 @@ public struct b3OpenGLVisualizerCameraInfo { public float[] m_target; } +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3UserConstraintState { + + /// double[6] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=6, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_appliedConstraintForces; + + /// int + public int m_numDofs; +} + public enum b3VREventType { /// VR_CONTROLLER_MOVE_EVENT -> 1 @@ -751,6 +825,9 @@ public struct b3VRControllerEvent { /// float public float m_analogAxis; + /// float[] + public float[] m_auxAnalogAxis; + /// int[64] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=64, ArraySubType=System.Runtime.InteropServices.UnmanagedType.I4)] public int[] m_buttons; @@ -965,6 +1042,41 @@ public struct b3VisualShapeInformation { public System.IntPtr m_visualShapeData; } +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential, CharSet=System.Runtime.InteropServices.CharSet.Ansi)] +public struct b3CollisionShapeData { + + /// int + public int m_objectUniqueId; + + /// int + public int m_linkIndex; + + /// int + public int m_collisionGeometryType; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_dimensions; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_localCollisionFrame; + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst=1024)] + public string m_meshAssetFileName; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3CollisionShapeInformation { + + /// int + public int m_numCollisionShapes; + + /// b3CollisionShapeData* + public System.IntPtr m_collisionShapeData; +} + public enum eLinkStateFlags { /// ACTUAL_STATE_COMPUTE_LINKVELOCITY -> 1 @@ -1036,6 +1148,33 @@ public enum EnumRenderer { ER_BULLET_HARDWARE_OPENGL = (1) << (17), } +public enum EnumRendererAuxFlags { + + /// ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX -> 1 + ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1, +} + +public enum EnumCalculateInverseKinematicsFlags { + + /// IK_DLS -> 0 + IK_DLS = 0, + + /// IK_SDLS -> 1 + IK_SDLS = 1, + + /// IK_HAS_TARGET_POSITION -> 16 + IK_HAS_TARGET_POSITION = 16, + + /// IK_HAS_TARGET_ORIENTATION -> 32 + IK_HAS_TARGET_ORIENTATION = 32, + + /// IK_HAS_NULL_SPACE_VELOCITY -> 64 + IK_HAS_NULL_SPACE_VELOCITY = 64, + + /// IK_HAS_JOINT_DAMPING -> 128 + IK_HAS_JOINT_DAMPING = 128, +} + public enum b3ConfigureDebugVisualizerEnum { /// COV_ENABLE_GUI -> 1 @@ -1058,12 +1197,22 @@ public enum b3ConfigureDebugVisualizerEnum { COV_ENABLE_KEYBOARD_SHORTCUTS, COV_ENABLE_MOUSE_PICKING, + + COV_ENABLE_Y_AXIS_UP, + + COV_ENABLE_TINY_RENDERER, + + COV_ENABLE_RGB_BUFFER_PREVIEW, + + COV_ENABLE_DEPTH_BUFFER_PREVIEW, + + COV_ENABLE_SEGMENTATION_MARK_PREVIEW, } public enum b3AddUserDebugItemEnum { - /// DEB_DEBUG_TEXT_USE_ORIENTATION -> 1 - DEB_DEBUG_TEXT_USE_ORIENTATION = 1, + /// DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA -> 1 + DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA = 1, /// DEB_DEBUG_TEXT_USE_TRUE_TYPE_FONTS -> 2 DEB_DEBUG_TEXT_USE_TRUE_TYPE_FONTS = 2, @@ -1112,6 +1261,15 @@ public enum eURDF_Flags { /// URDF_RESERVED -> 64 URDF_RESERVED = 64, + + /// URDF_USE_IMPLICIT_CYLINDER -> 128 + URDF_USE_IMPLICIT_CYLINDER = 128, + + /// URDF_GLOBAL_VELOCITIES_MB -> 256 + URDF_GLOBAL_VELOCITIES_MB = 256, + + /// MJCF_COLORS_FROM_FILE -> 512 + MJCF_COLORS_FROM_FILE = 512, } public enum eUrdfGeomTypes { @@ -1138,6 +1296,15 @@ public enum eUrdfCollisionFlags { GEOM_FORCE_CONCAVE_TRIMESH = 1, } +public enum eUrdfVisualFlags { + + /// GEOM_VISUAL_HAS_RGBA_COLOR -> 1 + GEOM_VISUAL_HAS_RGBA_COLOR = 1, + + /// GEOM_VISUAL_HAS_SPECULAR_COLOR -> 2 + GEOM_VISUAL_HAS_SPECULAR_COLOR = 2, +} + public enum eStateLoggingFlags { /// STATE_LOG_JOINT_MOTOR_TORQUES -> 1 @@ -1167,9 +1334,65 @@ public struct b3PluginArguments { /// int public int m_numFloats; - /// int[128] - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=128, ArraySubType=System.Runtime.InteropServices.UnmanagedType.I4)] - public int[] m_floats; + /// double[128] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=128, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_floats; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3PhysicsSimulationParameters { + + /// double + public double m_deltaTime; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_gravityAcceleration; + + /// int + public int m_numSimulationSubSteps; + + /// int + public int m_numSolverIterations; + + /// int + public int m_useRealTimeSimulation; + + /// int + public int m_useSplitImpulse; + + /// double + public double m_splitImpulsePenetrationThreshold; + + /// double + public double m_contactBreakingThreshold; + + /// int + public int m_internalSimFlags; + + /// double + public double m_defaultContactERP; + + /// int + public int m_collisionFilterMode; + + /// int + public int m_enableFileCaching; + + /// double + public double m_restitutionVelocityThreshold; + + /// double + public double m_defaultNonContactERP; + + /// double + public double m_frictionERP; + + /// int + public int m_enableConeFriction; + + /// int + public int m_deterministicOverlappingPairs; } [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] @@ -1194,9 +1417,8 @@ public struct b3SharedMemoryStatusHandle__ { } public partial class NativeMethods { - + const string dllName = "pybullet_vs2010_x64_release.dll"; - /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* ///key: int [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3ConnectSharedMemory")] @@ -1330,6 +1552,13 @@ public static extern System.IntPtr b3CreateCustomCommand(IntPtr physClient) ; public static extern void b3CustomCommandLoadPlugin(IntPtr commandHandle, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string pluginPath) ; + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///postFix: char* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CustomCommandLoadPluginSetPostFix")] +public static extern void b3CustomCommandLoadPluginSetPostFix(IntPtr commandHandle, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string postFix) ; + + /// Return Type: int ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetStatusPluginUniqueId")] @@ -1445,7 +1674,7 @@ public static extern int b3GetBodyUniqueId(IntPtr physClient, int serialIndex) ///bodyUniqueId: int ///info: b3BodyInfo* [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetBodyInfo")] - public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; +public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; /// Return Type: int @@ -1494,6 +1723,15 @@ public static extern System.IntPtr b3InitChangeDynamicsInfo(IntPtr physClient) public static extern int b3ChangeDynamicsInfoSetMass(IntPtr commandHandle, int bodyUniqueId, int linkIndex, double mass) ; + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///localInertiaDiagonal: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3ChangeDynamicsInfoSetLocalInertiaDiagonal")] +public static extern int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(IntPtr commandHandle, int bodyUniqueId, int linkIndex, ref double localInertiaDiagonal) ; + + /// Return Type: int ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///bodyUniqueId: int @@ -1651,6 +1889,20 @@ public static extern System.IntPtr b3InitRemoveUserConstraintCommand(IntPtr phy public static extern int b3GetNumUserConstraints(IntPtr physClient) ; + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///constraintUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3InitGetUserConstraintStateCommand")] +public static extern System.IntPtr b3InitGetUserConstraintStateCommand(IntPtr physClient, int constraintUniqueId) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///constraintState: b3UserConstraintState* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetStatusUserConstraintState")] +public static extern int b3GetStatusUserConstraintState(IntPtr statusHandle, ref b3UserConstraintState constraintState) ; + + /// Return Type: int ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* ///constraintUniqueId: int @@ -1753,6 +2005,13 @@ public static extern void b3UserDebugTextSetOptionFlags(IntPtr commandHandle, i public static extern void b3UserDebugTextSetOrientation(IntPtr commandHandle, ref double orientation) ; + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///replaceItem: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3UserDebugItemSetReplaceItemUniqueId")] +public static extern void b3UserDebugItemSetReplaceItemUniqueId(IntPtr commandHandle, int replaceItem) ; + + /// Return Type: void ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///objectUniqueId: int @@ -1905,6 +2164,13 @@ public static extern void b3RequestCameraImageSetShadow(IntPtr commandHandle, i public static extern void b3RequestCameraImageSelectRenderer(IntPtr commandHandle, int renderer) ; + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3RequestCameraImageSetFlags")] +public static extern void b3RequestCameraImageSetFlags(IntPtr commandHandle, int flags) ; + + /// Return Type: void ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* ///imageData: b3CameraImageData* @@ -2125,6 +2391,21 @@ public static extern System.IntPtr b3InitRequestVisualShapeInformation(IntPtr p public static extern void b3GetVisualShapeInformation(IntPtr physClient, ref b3VisualShapeInformation visualShapeInfo) ; + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + ///linkIndex: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3InitRequestCollisionShapeInformation")] +public static extern System.IntPtr b3InitRequestCollisionShapeInformation(IntPtr physClient, int bodyUniqueId, int linkIndex) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///collisionShapeInfo: b3CollisionShapeInformation* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetCollisionShapeInformation")] +public static extern void b3GetCollisionShapeInformation(IntPtr physClient, ref b3CollisionShapeInformation collisionShapeInfo) ; + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* ///filename: char* @@ -2285,6 +2566,33 @@ public static extern int b3PhysicsParamSetEnableFileCaching(IntPtr commandHandl public static extern int b3PhysicsParamSetRestitutionVelocityThreshold(IntPtr commandHandle, double restitutionVelocityThreshold) ; + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///enableConeFriction: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3PhysicsParamSetEnableConeFriction")] +public static extern int b3PhysicsParamSetEnableConeFriction(IntPtr commandHandle, int enableConeFriction) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///deterministicOverlappingPairs: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3PhysicsParameterSetDeterministicOverlappingPairs")] +public static extern int b3PhysicsParameterSetDeterministicOverlappingPairs(IntPtr commandHandle, int deterministicOverlappingPairs) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3InitRequestPhysicsParamCommand")] +public static extern System.IntPtr b3InitRequestPhysicsParamCommand(IntPtr physClient) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///params: b3PhysicsSimulationParameters* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetStatusPhysicsSimulationParameters")] +public static extern int b3GetStatusPhysicsSimulationParameters(IntPtr statusHandle, ref b3PhysicsSimulationParameters @params) ; + + /// Return Type: int ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///flags: int @@ -2358,6 +2666,38 @@ public static extern int b3LoadUrdfCommandSetFlags(IntPtr commandHandle, int fl public static extern int b3LoadUrdfCommandSetGlobalScaling(IntPtr commandHandle, double globalScaling) ; + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3SaveStateCommandInit")] +public static extern System.IntPtr b3SaveStateCommandInit(IntPtr physClient) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetStatusGetStateId")] +public static extern int b3GetStatusGetStateId(IntPtr statusHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadStateCommandInit")] +public static extern System.IntPtr b3LoadStateCommandInit(IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///stateId: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadStateSetStateId")] +public static extern int b3LoadStateSetStateId(IntPtr commandHandle, int stateId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///fileName: char* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadStateSetFileName")] +public static extern int b3LoadStateSetFileName(IntPtr commandHandle, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName) ; + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* ///fileName: char* @@ -2426,6 +2766,23 @@ public static extern System.IntPtr b3CalculateJacobianCommandInit(IntPtr physCl public static extern int b3GetStatusJacobian(IntPtr statusHandle, ref int dofCount, ref double linearJacobian, ref double angularJacobian) ; + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + ///jointPositionsQ: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CalculateMassMatrixCommandInit")] +public static extern System.IntPtr b3CalculateMassMatrixCommandInit(IntPtr physClient, int bodyIndex, ref double jointPositionsQ) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///dofCount: int* + ///massMatrix: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetStatusMassMatrix")] +public static extern int b3GetStatusMassMatrix(IntPtr physClient, IntPtr statusHandle, ref int dofCount, ref double massMatrix) ; + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* ///bodyIndex: int @@ -2485,6 +2842,13 @@ public static extern void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(In public static extern void b3CalculateInverseKinematicsSetJointDamping(IntPtr commandHandle, int numDof, ref double jointDampingCoeff) ; + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///solver: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CalculateInverseKinematicsSelectSolver")] +public static extern void b3CalculateInverseKinematicsSelectSolver(IntPtr commandHandle, int solver) ; + + /// Return Type: int ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* ///bodyUniqueId: int* @@ -2561,6 +2925,14 @@ public static extern int b3JointControlSetKp(IntPtr commandHandle, int dofIndex public static extern int b3JointControlSetKd(IntPtr commandHandle, int dofIndex, double value) ; + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///dofIndex: int + ///maximumVelocity: double + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3JointControlSetMaximumVelocity")] +public static extern int b3JointControlSetMaximumVelocity(IntPtr commandHandle, int dofIndex, double maximumVelocity) ; + + /// Return Type: int ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///dofIndex: int @@ -2666,6 +3038,85 @@ public static extern int b3GetStatusCollisionShapeUniqueId(IntPtr statusHandle) public static extern System.IntPtr b3CreateVisualShapeCommandInit(IntPtr physClient) ; + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///radius: double + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeAddSphere")] +public static extern int b3CreateVisualShapeAddSphere(IntPtr commandHandle, double radius) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///halfExtents: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeAddBox")] +public static extern int b3CreateVisualShapeAddBox(IntPtr commandHandle, ref double halfExtents) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///radius: double + ///height: double + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeAddCapsule")] +public static extern int b3CreateVisualShapeAddCapsule(IntPtr commandHandle, double radius, double height) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///radius: double + ///height: double + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeAddCylinder")] +public static extern int b3CreateVisualShapeAddCylinder(IntPtr commandHandle, double radius, double height) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///planeNormal: double* + ///planeConstant: double + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeAddPlane")] +public static extern int b3CreateVisualShapeAddPlane(IntPtr commandHandle, ref double planeNormal, double planeConstant) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///fileName: char* + ///meshScale: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeAddMesh")] +public static extern int b3CreateVisualShapeAddMesh(IntPtr commandHandle, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName, ref double meshScale) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///shapeIndex: int + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualSetFlag")] +public static extern void b3CreateVisualSetFlag(IntPtr commandHandle, int shapeIndex, int flags) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///shapeIndex: int + ///childPosition: double* + ///childOrientation: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeSetChildTransform")] +public static extern void b3CreateVisualShapeSetChildTransform(IntPtr commandHandle, int shapeIndex, ref double childPosition, ref double childOrientation) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///shapeIndex: int + ///specularColor: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeSetSpecularColor")] +public static extern void b3CreateVisualShapeSetSpecularColor(IntPtr commandHandle, int shapeIndex, ref double specularColor) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///shapeIndex: int + ///rgbaColor: double* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3CreateVisualShapeSetRGBAColor")] +public static extern void b3CreateVisualShapeSetRGBAColor(IntPtr commandHandle, int shapeIndex, ref double rgbaColor) ; + + /// Return Type: int ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3GetStatusVisualShapeUniqueId")] @@ -3000,29 +3451,30 @@ public static extern void b3ApplyExternalTorque(IntPtr commandHandle, int bodyU /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* - [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadBunnyCommandInit")] -public static extern System.IntPtr b3LoadBunnyCommandInit(IntPtr physClient) ; + ///fileName: char* + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadSoftBodyCommandInit")] +public static extern System.IntPtr b3LoadSoftBodyCommandInit(IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName) ; /// Return Type: int ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///scale: double - [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadBunnySetScale")] -public static extern int b3LoadBunnySetScale(IntPtr commandHandle, double scale) ; + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadSoftBodySetScale")] +public static extern int b3LoadSoftBodySetScale(IntPtr commandHandle, double scale) ; /// Return Type: int ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///mass: double - [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadBunnySetMass")] -public static extern int b3LoadBunnySetMass(IntPtr commandHandle, double mass) ; + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadSoftBodySetMass")] +public static extern int b3LoadSoftBodySetMass(IntPtr commandHandle, double mass) ; /// Return Type: int ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* ///collisionMargin: double - [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadBunnySetCollisionMargin")] -public static extern int b3LoadBunnySetCollisionMargin(IntPtr commandHandle, double collisionMargin) ; + [System.Runtime.InteropServices.DllImportAttribute(dllName, EntryPoint="b3LoadSoftBodySetCollisionMargin")] +public static extern int b3LoadSoftBodySetCollisionMargin(IntPtr commandHandle, double collisionMargin) ; /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__*