Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1731 from erwincoumans/master

Browse Source 
PyBullet / BulletRobotics: prepare for pdControlPlugin and collisionFilterPlugin, and allow to pick shared memory key in SHARED_MEMORY_SERVER
This commit is contained in:
erwincoumans
2018-06-05 18:01:12 -07:00
committed by GitHub
parent 7d0e632fb3 033835a0fa
commit e9b7ab1c59
41 changed files with 1009 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
Extras/BulletRobotics/CMakeLists.txt
View File

@@ -9,6 +9,12 @@ INCLUDE_DIRECTORIES(
)
SET(BulletRobotics_SRCS
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
@@ -91,8 +97,6 @@ SET(BulletRobotics_SRCS
../../examples/MultiThreading/b3PosixThreadSupport.cpp
../../examples/MultiThreading/b3Win32ThreadSupport.cpp
../../examples/MultiThreading/b3ThreadSupportInterface.cpp
../../examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
../../examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
)
IF(BUILD_CLSOCKET)
@@ -174,7 +178,8 @@ IF (INSTALL_EXTRA_LIBS)
../../examples/SharedMemory/PhysicsClientTCP_C_API.h
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
../../examples/SharedMemory/SharedMemoryPublic.h
../../examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
DESTINATION include/bullet
)
INSTALL(TARGETS

8
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -145,6 +145,8 @@ SET(BulletExampleBrowser_SRCS
../TinyRenderer/tgaimage.cpp
../TinyRenderer/our_gl.cpp
../TinyRenderer/TinyRenderer.cpp
../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
../SharedMemory/IKTrajectoryHelper.cpp
@@ -175,10 +177,12 @@ SET(BulletExampleBrowser_SRCS
../SharedMemory/SharedMemoryCommands.h
../SharedMemory/SharedMemoryPublic.h
../SharedMemory/b3PluginManager.cpp
../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h
../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../RobotSimulator/b3RobotSimulatorClientAPI.cpp
../RobotSimulator/b3RobotSimulatorClientAPI.h
../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
../BasicDemo/BasicExample.cpp
../BasicDemo/BasicExample.h
../InverseDynamics/InverseDynamicsExample.cpp

8
examples/ExampleBrowser/premake4.lua
View File

@@ -117,8 +117,14 @@ project "App_BulletExampleBrowser"
"../SharedMemory/b3PluginManager.cpp",
"../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
"../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../SharedMemory/SharedMemoryCommands.h",
"../SharedMemory/SharedMemoryPublic.h",
"../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",
"../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h",
"../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../MultiThreading/MultiThreadingExample.cpp",
"../MultiThreading/b3PosixThreadSupport.cpp",
"../MultiThreading/b3Win32ThreadSupport.cpp",
@@ -127,8 +133,6 @@ project "App_BulletExampleBrowser"
"../InverseDynamics/InverseDynamicsExample.h",
"../RobotSimulator/b3RobotSimulatorClientAPI.cpp",
"../RobotSimulator/b3RobotSimulatorClientAPI.h",
"../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp",
"../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h",
"../BasicDemo/BasicExample.*",
"../Tutorial/*",
"../ExtendedTutorials/*",

12
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -595,11 +595,13 @@ void ConvertURDF2BulletInternal(
}
} else
{
//todo: fix the crash it can cause
//if (cache.m_bulletMultiBody->getBaseMass()==0)
//{
// col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);//:CF_STATIC_OBJECT);
//}
if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumLinks()==0)
{
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
}
cache.m_bulletMultiBody->setBaseCollider(col);
}
}

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -1,7 +1,7 @@
#include "b3RobotSimulatorClientAPI.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "b3RobotSimulatorClientAPI_InternalData.h"
#include "../SharedMemory/b3RobotSimulatorClientAPI_InternalData.h"
#ifdef BT_ENABLE_ENET
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
#endif //PHYSICS_UDP

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -1,7 +1,7 @@
#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H
#define B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H
#include "b3RobotSimulatorClientAPI_NoGUI.h"
#include "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h"
///The b3RobotSimulatorClientAPI_GUI is pretty much the C++ version of pybullet

11
examples/RobotSimulator/premake4.lua
View File

@@ -1,6 +1,13 @@
myfiles =
{
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
@@ -191,8 +198,6 @@ if not _OPTIONS["no-enet"] then
"RobotSimulatorMain.cpp",
"b3RobotSimulatorClientAPI.cpp",
"b3RobotSimulatorClientAPI.h",
"b3RobotSimulatorClientAPI_NoGUI.cpp",
"b3RobotSimulatorClientAPI_NoGUI.h",
"MinitaurSetup.cpp",
"MinitaurSetup.h",
myfiles
@@ -283,8 +288,6 @@ project ("App_VRGloveHandSimulator")
"VRGloveSimulatorMain.cpp",
"b3RobotSimulatorClientAPI.cpp",
"b3RobotSimulatorClientAPI.h",
"b3RobotSimulatorClientAPI_NoGUI.cpp",
"b3RobotSimulatorClientAPI_NoGUI.h",
myfiles
}

4
examples/SharedMemory/CMakeLists.txt
View File

@@ -1,5 +1,9 @@
SET(SharedMemory_SRCS
plugins/pdControlPlugin/pdControlPlugin.cpp
plugins/pdControlPlugin/pdControlPlugin.h
b3RobotSimulatorClientAPI_NoDirect.cpp
b3RobotSimulatorClientAPI_NoDirect.h
IKTrajectoryHelper.cpp
IKTrajectoryHelper.h
PhysicsClient.cpp

60
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -45,6 +45,9 @@
#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
#include "plugins/pdControlPlugin/pdControlPlugin.h"
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifdef STATIC_LINK_VR_PLUGIN
#include "plugins/vrSyncPlugin/vrSyncPlugin.h"
@@ -102,7 +105,6 @@ struct UrdfLinkNameMapUtil
}
virtual ~UrdfLinkNameMapUtil()
{
delete m_memSerializer;
}
};
@@ -1596,7 +1598,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
@@ -1647,7 +1649,7 @@ struct PhysicsServerCommandProcessorInternalData
btVector3 m_hitPos;
btScalar m_oldPickingDist;
bool m_prevCanSleep;
int m_pdControlPlugin;
#ifdef B3_ENABLE_TINY_AUDIO
b3SoundEngine m_soundEngine;
#endif
@@ -1679,15 +1681,23 @@ struct PhysicsServerCommandProcessorInternalData
m_verboseOutput(false),
m_pickedBody(0),
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
m_pickingMultiBodyPoint2Point(0),
m_pdControlPlugin(-1)
{
{
//register static plugins:
#ifdef STATIC_LINK_VR_PLUGIN
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin,preTickPluginCallback_vrSyncPlugin,0,0);
m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0);
#endif //STATIC_LINK_VR_PLUGIN
#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
{
m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0);
}
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin,0,0,getRenderInterface_tinyRendererPlugin);
m_pluginManager.selectPluginRenderer(renderPluginId);
@@ -2471,12 +2481,13 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
}
m_data->m_worldImporters.clear();
#ifdef ENABLE_LINK_MAPPER
for (int i=0;i<m_data->m_urdfLinkNameMapper.size();i++)
{
delete m_data->m_urdfLinkNameMapper[i];
}
m_data->m_urdfLinkNameMapper.clear();
#endif //ENABLE_LINK_MAPPER
for (int i=0;i<m_data->m_strings.size();i++)
{
@@ -2999,10 +3010,12 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
btMultiBody* mb = bodyHandle? bodyHandle->m_multiBody:0;
if (mb)
{
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util);
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
util->m_mb = mb;
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
@@ -3035,9 +3048,11 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
btRigidBody* rb = bodyHandle? bodyHandle->m_rigidBody :0;
if (rb)
{
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_data->m_urdfLinkNameMapper.push_back(util);
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody,bodyHandle->m_bodyName.c_str());
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
@@ -4853,7 +4868,7 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S
if (!userData) {
return hasStatus;
}
btAssert(bufferSizeInBytes >= userData->m_bytes.size())
btAssert(bufferSizeInBytes >= userData->m_bytes.size());
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs;
serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type;
serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size();
@@ -4949,6 +4964,13 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{
/* case CONTROL_MODE_PD:
{
b3PluginArguments args;
m_data->m_pluginManager.executePluginCommand(pdControlPlugin, args);
//#p.executePluginCommand(plugin ,"r2d2.urdf", [1,2,3],[50.0,3.3])
}
*/
case CONTROL_MODE_TORQUE:
{
if (m_data->m_verboseOutput)
@@ -6031,10 +6053,8 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
@@ -6102,12 +6122,14 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
#ifdef ENABLE_LINK_MAPPER
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
#endif
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
@@ -10134,4 +10156,4 @@ const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() co
void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
{
gVRTeleportOrn = vrTeleportOrn;
}
}

8
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -271,10 +271,13 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingEx
return (b3PhysicsClientHandle ) cl;
}
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr)
extern int gSharedMemoryKey;
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr, int sharedMemoryKey)
{
static DummyGUIHelper noGfx;
gSharedMemoryKey = sharedMemoryKey;
GUIHelperInterface* guiHelper = (GUIHelperInterface*) guiHelperPtr;
if (!guiHelper)
{
@@ -283,7 +286,8 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistin
bool useInprocessMemory = false;
bool skipGraphicsUpdate = true;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper, useInprocessMemory, skipGraphicsUpdate);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
cl->setSharedMemoryKey(sharedMemoryKey+1);
cl->connect();
return (b3PhysicsClientHandle ) cl;
}

2
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -18,7 +18,7 @@ B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMain
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
//create a shared memory physics server, with a DummyGUIHelper (no graphics)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(void* guiHelperPtr, int sharedMemoryKey);
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);

0
examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h → examples/SharedMemory/b3RobotSimulatorClientAPI_InternalData.h
View File

279
examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp → examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -1,20 +1,9 @@
#include "b3RobotSimulatorClientAPI_NoGUI.h"
#include "b3RobotSimulatorClientAPI_NoDirect.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "PhysicsClientC_API.h"
#include "b3RobotSimulatorClientAPI_InternalData.h"
#ifdef BT_ENABLE_ENET
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
#endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
#endif //PHYSICS_TCP
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
static void scalarToDouble3(btScalar a[3], double b[3])
@@ -27,110 +16,32 @@ static void scalarToDouble3(btScalar a[3], double b[3])
static void scalarToDouble4(btScalar a[4], double b[4])
{
for (int i = 0; i < 4; i++)
for (int i = 0; i < 4; i++)
{
b[i] = a[i];
}
}
b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI()
b3RobotSimulatorClientAPI_NoDirect::b3RobotSimulatorClientAPI_NoDirect()
{
m_data = new b3RobotSimulatorClientAPI_InternalData();
}
b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI()
b3RobotSimulatorClientAPI_NoDirect::~b3RobotSimulatorClientAPI_NoDirect()
{
delete m_data;
}
bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey)
{
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
bool b3RobotSimulatorClientAPI_NoDirect::isConnected() const
{
return (m_data->m_physicsClientHandle != 0);
}
void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec)
void b3RobotSimulatorClientAPI_NoDirect::setTimeOut(double timeOutInSec)
{
if (!isConnected())
{
@@ -142,19 +53,22 @@ void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec)
}
void b3RobotSimulatorClientAPI_NoGUI::disconnect()
void b3RobotSimulatorClientAPI_NoDirect::disconnect()
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
#ifndef BT_DISABLE_PHYSICS_DIRECT
b3DisconnectSharedMemory(m_data->m_physicsClientHandle);
m_data->m_physicsClientHandle = 0;
#endif
}
void b3RobotSimulatorClientAPI_NoGUI::syncBodies()
void b3RobotSimulatorClientAPI_NoDirect::syncBodies()
{
if (!isConnected())
{
@@ -171,7 +85,7 @@ void b3RobotSimulatorClientAPI_NoGUI::syncBodies()
statusType = b3GetStatusType(statusHandle);
}
void b3RobotSimulatorClientAPI_NoGUI::resetSimulation()
void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
{
if (!isConnected())
{
@@ -183,7 +97,7 @@ void b3RobotSimulatorClientAPI_NoGUI::resetSimulation()
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
}
bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const
bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
{
if (!isConnected())
{
@@ -192,7 +106,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const
return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
}
void b3RobotSimulatorClientAPI_NoGUI::stepSimulation()
void b3RobotSimulatorClientAPI_NoDirect::stepSimulation()
{
if (!isConnected())
{
@@ -210,7 +124,7 @@ void b3RobotSimulatorClientAPI_NoGUI::stepSimulation()
}
}
void b3RobotSimulatorClientAPI_NoGUI::setGravity(const btVector3& gravityAcceleration)
void b3RobotSimulatorClientAPI_NoDirect::setGravity(const btVector3& gravityAcceleration)
{
if (!isConnected())
{
@@ -226,14 +140,14 @@ void b3RobotSimulatorClientAPI_NoGUI::setGravity(const btVector3& gravityAcceler
// btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
btQuaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const btVector3& rollPitchYaw)
btQuaternion b3RobotSimulatorClientAPI_NoDirect::getQuaternionFromEuler(const btVector3& rollPitchYaw)
{
btQuaternion q;
q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
return q;
}
btVector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const btQuaternion& quat)
btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQuaternion& quat)
{
btScalar roll,pitch,yaw;
quat.getEulerZYX(yaw,pitch,roll);
@@ -241,7 +155,7 @@ btVector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const btQuater
return rpy2;
}
int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
{
int robotUniqueId = -1;
@@ -279,7 +193,7 @@ int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const
return robotUniqueId;
}
bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
{
if (!isConnected())
{
@@ -310,7 +224,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3Ro
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
bool b3RobotSimulatorClientAPI_NoDirect::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
{
if (!isConnected())
{
@@ -340,7 +254,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args)
bool b3RobotSimulatorClientAPI_NoDirect::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args)
{
if (!isConnected())
{
@@ -372,7 +286,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3Rob
return statusOk;
}
bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo)
bool b3RobotSimulatorClientAPI_NoDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo)
{
if (!isConnected())
{
@@ -384,7 +298,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3Bod
return (result != 0);
}
bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const
bool b3RobotSimulatorClientAPI_NoDirect::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const
{
if (!isConnected())
{
@@ -422,7 +336,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniq
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation)
bool b3RobotSimulatorClientAPI_NoDirect::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation)
{
if (!isConnected())
{
@@ -444,7 +358,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUn
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const
bool b3RobotSimulatorClientAPI_NoDirect::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const
{
if (!isConnected())
{
@@ -478,7 +392,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, btVector
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const
bool b3RobotSimulatorClientAPI_NoDirect::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const
{
if (!isConnected())
{
@@ -503,7 +417,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const
return true;
}
void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags)
void b3RobotSimulatorClientAPI_NoDirect::setInternalSimFlags(int flags)
{
if (!isConnected())
{
@@ -519,7 +433,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags)
}
void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeSimulation)
void b3RobotSimulatorClientAPI_NoDirect::setRealTimeSimulation(bool enableRealTimeSimulation)
{
if (!isConnected())
{
@@ -537,7 +451,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeS
btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const
int b3RobotSimulatorClientAPI_NoDirect::getNumJoints(int bodyUniqueId) const
{
if (!isConnected())
{
@@ -547,7 +461,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const
return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
}
bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
bool b3RobotSimulatorClientAPI_NoDirect::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{
if (!isConnected())
{
@@ -557,7 +471,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIn
return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
}
int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
{
if (!isConnected())
{
@@ -579,7 +493,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int p
return -1;
}
int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointInfo* jointInfo)
int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3JointInfo* jointInfo)
{
if (!isConnected())
@@ -608,7 +522,7 @@ int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointI
return statusType;
}
void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId)
void b3RobotSimulatorClientAPI_NoDirect::removeConstraint(int constraintId)
{
if (!isConnected())
{
@@ -621,7 +535,7 @@ void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId)
}
bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
bool b3RobotSimulatorClientAPI_NoDirect::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{
if (!isConnected())
{
@@ -641,7 +555,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointI
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointStates2& state)
bool b3RobotSimulatorClientAPI_NoDirect::getJointStates(int bodyUniqueId, b3JointStates2& state)
{
if (!isConnected())
{
@@ -700,7 +614,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointSt
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
bool b3RobotSimulatorClientAPI_NoDirect::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
{
if (!isConnected())
{
@@ -726,7 +640,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::resetJointState(int bodyUniqueId, int join
return false;
}
void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
void b3RobotSimulatorClientAPI_NoDirect::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
{
if (!isConnected())
{
@@ -781,7 +695,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int
}
}
void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations)
void b3RobotSimulatorClientAPI_NoDirect::setNumSolverIterations(int numIterations)
{
if (!isConnected())
{
@@ -795,7 +709,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations)
btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double threshold)
void b3RobotSimulatorClientAPI_NoDirect::setContactBreakingThreshold(double threshold)
{
if (!isConnected())
{
@@ -810,7 +724,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double thresho
}
void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds)
void b3RobotSimulatorClientAPI_NoDirect::setTimeStep(double timeStepInSeconds)
{
if (!isConnected())
{
@@ -825,7 +739,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds)
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps)
void b3RobotSimulatorClientAPI_NoDirect::setNumSimulationSubSteps(int numSubSteps)
{
if (!isConnected())
{
@@ -839,7 +753,7 @@ void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps)
btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
{
if (!isConnected())
{
@@ -892,7 +806,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3
return result;
}
bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
{
if (!isConnected())
{
@@ -912,7 +826,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int link
}
bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
{
bool computeLinkVelocity = true;
bool computeForwardKinematics = true;
@@ -920,7 +834,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkInd
return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState);
}
bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -946,7 +860,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkInd
return false;
}
void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable)
void b3RobotSimulatorClientAPI_NoDirect::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable)
{
if (!isConnected())
{
@@ -958,7 +872,7 @@ void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugV
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
void b3RobotSimulatorClientAPI_NoDirect::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
{
vrEventsData->m_numControllerEvents = 0;
vrEventsData->m_controllerEvents = 0;
@@ -974,7 +888,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,i
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
}
void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
void b3RobotSimulatorClientAPI_NoDirect::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
{
keyboardEventsData->m_numKeyboardEvents = 0;
keyboardEventsData->m_keyboardEvents = 0;
@@ -989,7 +903,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* ke
b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
}
int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
int b3RobotSimulatorClientAPI_NoDirect::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
{
if (!isConnected())
{
@@ -1024,7 +938,7 @@ int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggin
return loggingUniqueId;
}
void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId)
void b3RobotSimulatorClientAPI_NoDirect::stopStateLogging(int stateLoggerUniqueId)
{
if (!isConnected())
{
@@ -1039,7 +953,7 @@ void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId)
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos)
void b3RobotSimulatorClientAPI_NoDirect::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos)
{
if (!isConnected())
{
@@ -1060,7 +974,7 @@ void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDi
}
}
void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName, int durationInMicroSeconds)
{
if (!isConnected())
{
@@ -1076,7 +990,7 @@ void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string& pr
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
{
if (!isConnected())
{
@@ -1091,7 +1005,7 @@ void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName,
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}
void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEventsData)
void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
{
mouseEventsData->m_numMouseEvents = 0;
mouseEventsData->m_mouseEvents = 0;
@@ -1106,7 +1020,7 @@ void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEve
}
bool b3RobotSimulatorClientAPI_NoGUI::getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, struct b3CameraImageData &imageData)
bool b3RobotSimulatorClientAPI_NoDirect::getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, struct b3CameraImageData &imageData)
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -1172,7 +1086,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getCameraImage(int width, int height, stru
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities,
bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities,
double *jointAccelerations, double *jointForcesOutput)
{
if (!isConnected()) {
@@ -1204,7 +1118,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId,
return false;
}
int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const
int b3RobotSimulatorClientAPI_NoDirect::getNumBodies() const
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -1213,7 +1127,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const
return b3GetNumBodies(m_data->m_physicsClientHandle);
}
int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const
int b3RobotSimulatorClientAPI_NoDirect::getBodyUniqueId(int bodyId) const
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -1222,7 +1136,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const
return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId);
}
bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId)
bool b3RobotSimulatorClientAPI_NoDirect::removeBody(int bodyUniqueId)
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -1246,7 +1160,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId)
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) {
bool b3RobotSimulatorClientAPI_NoDirect::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) {
if (!isConnected()) {
b3Warning("Not connected");
return false;
@@ -1282,7 +1196,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int link
}
bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args)
bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1331,7 +1245,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkI
return true;
}
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) {
int b3RobotSimulatorClientAPI_NoDirect::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) {
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
b3Warning("Not connected to physics server.");
@@ -1354,7 +1268,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, dou
}
double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) {
double b3RobotSimulatorClientAPI_NoDirect::readUserDebugParameter(int itemUniqueId) {
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
b3Warning("Not connected to physics server.");
@@ -1380,7 +1294,7 @@ double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId)
}
bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) {
bool b3RobotSimulatorClientAPI_NoDirect::removeUserDebugItem(int itemUniqueId) {
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
b3Warning("Not connected to physics server.");
@@ -1397,7 +1311,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) {
}
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1429,7 +1343,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPo
return -1;
}
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
{
double dposXYZ[3];
dposXYZ[0] = textPosition.x();
@@ -1439,7 +1353,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &tex
return addUserDebugText(text, &dposXYZ[0], args);
}
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
int b3RobotSimulatorClientAPI_NoDirect::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1467,7 +1381,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *t
return -1;
}
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
int b3RobotSimulatorClientAPI_NoDirect::addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
{
double dfromXYZ[3];
double dtoXYZ[3];
@@ -1483,7 +1397,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(btVector3 &fromXYZ, btVect
bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args)
bool b3RobotSimulatorClientAPI_NoDirect::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1565,7 +1479,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId
}
bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1636,7 +1550,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi
}
bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags)
bool b3RobotSimulatorClientAPI_NoDirect::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1652,7 +1566,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags)
bool b3RobotSimulatorClientAPI_NoDirect::applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags)
{
double dforce[3];
double dposition[3];
@@ -1669,7 +1583,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int
}
bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags)
bool b3RobotSimulatorClientAPI_NoDirect::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1685,7 +1599,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, in
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags)
bool b3RobotSimulatorClientAPI_NoDirect::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags)
{
double dtorque[3];
@@ -1697,7 +1611,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, in
}
bool b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable)
bool b3RobotSimulatorClientAPI_NoDirect::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1723,7 +1637,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniq
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo)
bool b3RobotSimulatorClientAPI_NoDirect::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1744,7 +1658,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVi
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo)
bool b3RobotSimulatorClientAPI_NoDirect::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1780,7 +1694,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGe
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo)
bool b3RobotSimulatorClientAPI_NoDirect::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1815,7 +1729,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGe
}
bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData)
bool b3RobotSimulatorClientAPI_NoDirect::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1834,7 +1748,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, dou
}
bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData)
bool b3RobotSimulatorClientAPI_NoDirect::getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData)
{
double daabbMin[3];
double daabbMax[3];
@@ -1852,7 +1766,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(btVector3 &aabbMin,
bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax)
bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1891,7 +1805,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, d
return false;
}
bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax)
bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax)
{
double daabbMin[3];
double daabbMax[3];
@@ -1910,7 +1824,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, b
}
int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args)
int b3RobotSimulatorClientAPI_NoDirect::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1962,7 +1876,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct
return -1;
}
int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args)
int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -2035,7 +1949,7 @@ int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCrea
return -1;
}
int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const
int b3RobotSimulatorClientAPI_NoDirect::getNumConstraints() const
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -2044,7 +1958,7 @@ int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const
return b3GetNumUserConstraints(m_data->m_physicsClientHandle);
}
int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex)
int b3RobotSimulatorClientAPI_NoDirect::getConstraintUniqueId(int serialIndex)
{
if (!isConnected()) {
b3Warning("Not connected");
@@ -2057,17 +1971,24 @@ int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex)
void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* guiHelper)
void b3RobotSimulatorClientAPI_NoDirect::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
m_data->m_guiHelper = guiHelper;
}
struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper()
struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoDirect::getGuiHelper()
{
return m_data->m_guiHelper;
}
bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, int linkIndex,
void b3RobotSimulatorClientAPI_NoDirect::setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data)
{
*m_data = *data;
}
bool b3RobotSimulatorClientAPI_NoDirect::getCollisionShapeData(int bodyUniqueId, int linkIndex,
b3CollisionShapeInformation &collisionShapeInfo)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
@@ -2092,7 +2013,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, in
return true;
}
bool b3RobotSimulatorClientAPI_NoGUI::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {

26
examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h → examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -1,5 +1,9 @@
#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H
#define B3_ROBOT_SIMULATOR_CLIENT_API_H
#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H
#define B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
#include "SharedMemoryPublic.h"
#include "LinearMath/btVector3.h"
@@ -404,11 +408,7 @@ struct b3RobotSimulatorCreateMultiBodyArgs
};
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
class b3RobotSimulatorClientAPI_NoGUI
class b3RobotSimulatorClientAPI_NoDirect
{
protected:
@@ -416,10 +416,11 @@ protected:
public:
b3RobotSimulatorClientAPI_NoGUI();
virtual ~b3RobotSimulatorClientAPI_NoGUI();
b3RobotSimulatorClientAPI_NoDirect();
virtual ~b3RobotSimulatorClientAPI_NoDirect();
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
//No 'connect', use setInternalData to bypass the connect method, pass an existing client
virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data);
void disconnect();
@@ -580,6 +581,9 @@ public:
bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H

112
examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp Normal file
View File

@@ -0,0 +1,112 @@
#include "b3RobotSimulatorClientAPI_NoGUI.h"
#include "PhysicsClientC_API.h"
#include "b3RobotSimulatorClientAPI_InternalData.h"
#ifdef BT_ENABLE_ENET
#include "PhysicsClientUDP_C_API.h"
#endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET
#include "PhysicsClientTCP_C_API.h"
#endif //PHYSICS_TCP
#ifndef BT_DISABLE_PHYSICS_DIRECT
#include "PhysicsDirectC_API.h"
#endif //BT_DISABLE_PHYSICS_DIRECT
#include "SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI()
{
}
b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI()
{
}
bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey)
{
if (m_data->m_physicsClientHandle)
{
b3Warning("Already connected, disconnect first.");
return false;
}
b3PhysicsClientHandle sm = 0;
int udpPort = 1234;
int tcpPort = 6667;
int key = SHARED_MEMORY_KEY;
bool connected = false;
switch (mode)
{
case eCONNECT_DIRECT:
{
#ifndef BT_DISABLE_PHYSICS_DIRECT
sm = b3ConnectPhysicsDirect();
#endif //BT_DISABLE_PHYSICS_DIRECT
break;
}
case eCONNECT_SHARED_MEMORY:
{
if (portOrKey >= 0)
{
key = portOrKey;
}
sm = b3ConnectSharedMemory(key);
break;
}
case eCONNECT_UDP:
{
if (portOrKey >= 0)
{
udpPort = portOrKey;
}
#ifdef BT_ENABLE_ENET
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
#else
b3Warning("UDP is not enabled in this build");
#endif //BT_ENABLE_ENET
break;
}
case eCONNECT_TCP:
{
if (portOrKey >= 0)
{
tcpPort = portOrKey;
}
#ifdef BT_ENABLE_CLSOCKET
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
#else
b3Warning("TCP is not enabled in this pybullet build");
#endif //BT_ENABLE_CLSOCKET
break;
}
default:
{
b3Warning("connectPhysicsServer unexpected argument");
}
};
if (sm)
{
m_data->m_physicsClientHandle = sm;
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
{
disconnect();
return false;
}
return true;
}
return false;
}

22
examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H
#define B3_ROBOT_SIMULATOR_CLIENT_API_H
#include "b3RobotSimulatorClientAPI_NoDirect.h"
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
class b3RobotSimulatorClientAPI_NoGUI : public b3RobotSimulatorClientAPI_NoDirect
{
public:
b3RobotSimulatorClientAPI_NoGUI();
virtual ~b3RobotSimulatorClientAPI_NoGUI();
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
};
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

98
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp Normal file
View File

@@ -0,0 +1,98 @@
//tinyRendererPlugin implements the TinyRenderer as a plugin
//it is statically linked when using preprocessor #define STATIC_LINK_VR_PLUGIN
//otherwise you can dynamically load it using pybullet.loadPlugin
#include "collisionFilterPlugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
struct MyClass
{
int m_testData;
MyClass()
:m_testData(42)
{
}
virtual ~MyClass()
{
}
};
B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
printf("hi!\n");
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
//apply pd control here, apply forces using the PD gains
return 0;
}
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
//set the PD gains
printf("text argument:%s\n",arguments->m_text);
printf("int args: [");
for (int i=0;i<arguments->m_numInts;i++)
{
printf("%d", arguments->m_ints[i]);
if ((i+1)<arguments->m_numInts)
{
printf(",");
}
}
printf("]\nfloat args: [");
for (int i=0;i<arguments->m_numFloats;i++)
{
printf("%f", arguments->m_floats[i]);
if ((i+1)<arguments->m_numFloats)
{
printf(",");
}
}
printf("]\n");
MyClass* obj = (MyClass*) context->m_userPointer;
b3SharedMemoryStatusHandle statusHandle;
int statusType = -1;
int bodyUniqueId = -1;
b3SharedMemoryCommandHandle command =
b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
}
return bodyUniqueId;
}
B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
printf("bye!\n");
}

25
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef COLLISION_FILTER_PLUGIN_H
#define COLLISION_FILTER_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define COLLISION_FILTER_PLUGIN_H

42
examples/SharedMemory/plugins/collisionFilterPlugin/premake4.lua Normal file
View File

@@ -0,0 +1,42 @@
project ("pybullet_collisionFilterPlugin")
language "C++"
kind "SharedLib"
includedirs {".","../../../../src", "../../../../examples",
"../../../ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
if os.is("MacOSX") then
-- targetextension {"so"}
links{"Cocoa.framework","Python"}
end
files {
"collisionFilterPlugin.cpp",
"../../PhysicsClient.cpp",
"../../PhysicsClient.h",
"../../PhysicsClientSharedMemory.cpp",
"../../PhysicsClientSharedMemory.h",
"../../PhysicsClientSharedMemory_C_API.cpp",
"../../PhysicsClientSharedMemory_C_API.h",
"../../PhysicsClientC_API.cpp",
"../../PhysicsClientC_API.h",
"../../Win32SharedMemory.cpp",
"../../Win32SharedMemory.h",
"../../PosixSharedMemory.cpp",
"../../PosixSharedMemory.h",
"../../../Utils/b3Clock.cpp",
"../../../Utils/b3Clock.h",
"../../../Utils/b3ResourcePath.cpp",
"../../../Utils/b3ResourcePath.h",
}

174
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp Normal file
View File

@@ -0,0 +1,174 @@
#include "pdControlPlugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "../../b3RobotSimulatorClientAPI_NoDirect.h"
#include "../../b3RobotSimulatorClientAPI_InternalData.h"
struct MyPDControl
{
int m_objectUniqueId;
int m_linkIndex;
btScalar m_desiredPosition;
btScalar m_desiredVelocity;
btScalar m_kd;
btScalar m_kp;
btScalar m_maxForce;
};
struct MyPDControlContainer
{
int m_testData;
btAlignedObjectArray<MyPDControl> m_controllers;
b3RobotSimulatorClientAPI_NoDirect m_api;
MyPDControlContainer()
:m_testData(42)
{
}
virtual ~MyPDControlContainer()
{
}
};
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = new MyPDControlContainer();
b3RobotSimulatorClientAPI_InternalData data;
data.m_physicsClientHandle = context->m_physClient;
data.m_guiHelper = 0;
obj->m_api.setInternalData(&data);
context->m_userPointer = obj;
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
{
//apply pd control here, apply forces using the PD gains
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
const MyPDControl& pdControl = obj->m_controllers[i];
int dof1 = 0;
b3JointSensorState actualState;
if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
{
if (pdControl.m_maxForce>0)
{
//compute torque
btScalar qActual = actualState.m_jointPosition;
btScalar qdActual = actualState.m_jointVelocity;
btScalar positionError = (pdControl.m_desiredPosition -qActual);
double desiredVelocity = 0;
btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
btClamp(force,-pdControl.m_maxForce,pdControl.m_maxForce);
//apply torque
b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE);
args.m_maxTorqueValue = force;
obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args);
}
}
}
return 0;
}
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
obj->m_api.syncBodies();
int numObj = obj->m_api.getNumBodies();
//printf("numObj = %d\n", numObj);
//protocol:
//first int is the type of command
//second int is body unique id
//third int is link index
if (arguments->m_numInts != 3)
return -1;
switch (arguments->m_ints[0])
{
case eSetPDControl:
{
if (arguments->m_numFloats < 5)
return -1;
MyPDControl controller;
controller.m_desiredPosition = arguments->m_floats[0];
controller.m_desiredVelocity = arguments->m_floats[1];
controller.m_kd = arguments->m_floats[2];
controller.m_kp = arguments->m_floats[3];
controller.m_maxForce = arguments->m_floats[4];
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
int foundIndex = -1;
for (int i = 0; i < obj->m_controllers.size(); i++)
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers[i] = controller;
foundIndex=i;
}
}
if (foundIndex<0)
{
obj->m_controllers.push_back(controller);
}
break;
}
case eRemovePDControl:
{
MyPDControl controller;
controller.m_objectUniqueId = arguments->m_ints[1];
controller.m_linkIndex = arguments->m_ints[2];
for (int i = 0; i < obj->m_controllers.size(); i++)
{
if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
{
obj->m_controllers.removeAtIndex(i);
break;
}
}
break;
}
default:
{
return -1;
}
}
int result = 42;
return result;
}
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context)
{
MyPDControlContainer* obj = (MyPDControlContainer*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
}

31
examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h Normal file
View File

@@ -0,0 +1,31 @@
#ifndef PID_CONTROL_PLUGIN_H
#define PID_CONTROL_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//the following 3 APIs are required
B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
///
enum PDControlCommandEnum
{
eSetPDControl = 1,
eRemovePDControl = 2,
};
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define PID_CONTROL_PLUGIN_H

44
examples/SharedMemory/plugins/pdControlPlugin/premake4.lua Normal file
View File

@@ -0,0 +1,44 @@
project ("pybullet_pdControlPlugin")
language "C++"
kind "SharedLib"
includedirs {".","../../../../src", "../../../../examples",
"../../../ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
if os.is("MacOSX") then
-- targetextension {"so"}
links{"Cocoa.framework","Python"}
end
files {
"pdControlPlugin.cpp",
"../../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../b3RobotSimulatorClientAPI_NoDirect.h",
"../../PhysicsClient.cpp",
"../../PhysicsClient.h",
"../../PhysicsClientSharedMemory.cpp",
"../../PhysicsClientSharedMemory.h",
"../../PhysicsClientSharedMemory_C_API.cpp",
"../../PhysicsClientSharedMemory_C_API.h",
"../../PhysicsClientC_API.cpp",
"../../PhysicsClientC_API.h",
"../../Win32SharedMemory.cpp",
"../../Win32SharedMemory.h",
"../../PosixSharedMemory.cpp",
"../../PosixSharedMemory.h",
"../../../Utils/b3Clock.cpp",
"../../../Utils/b3Clock.h",
"../../../Utils/b3ResourcePath.cpp",
"../../../Utils/b3ResourcePath.h",
}

56
examples/SharedMemory/plugins/testPlugin/testplugin.cpp
View File

@@ -29,7 +29,7 @@ struct MyClass
}
};
B3_SHARED_API int initPlugin(struct b3PluginContext* context)
B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
@@ -39,67 +39,23 @@ B3_SHARED_API int initPlugin(struct b3PluginContext* context)
}
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
{
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient);
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
{
struct b3VREventsData vrEvents;
int i = 0;
b3GetVREventsData(context->m_physClient, &vrEvents);
if (vrEvents.m_numControllerEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d VR controller events!\n", vrEvents.m_numControllerEvents);
}
}
}
{
b3KeyboardEventsData keyboardEventsData;
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(context->m_physClient);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
b3GetKeyboardEventsData(context->m_physClient, &keyboardEventsData);
if (keyboardEventsData.m_numKeyboardEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d keyboard events\n", keyboardEventsData.m_numKeyboardEvents);
}
}
{
b3MouseEventsData mouseEventsData;
b3SharedMemoryCommandHandle commandHandle = b3RequestMouseEventsCommandInit(context->m_physClient);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
b3GetMouseEventsData(context->m_physClient, &mouseEventsData);
if (mouseEventsData.m_numMouseEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d mouse events\n", mouseEventsData.m_numMouseEvents);
}
}
return 0;
}
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context)
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
printf("text argument:%s\n",arguments->m_text);
printf("int args: [");
for (int i=0;i<arguments->m_numInts;i++)
@@ -140,7 +96,7 @@ B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const st
}
B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;

11
examples/SharedMemory/plugins/testPlugin/testplugin.h
View File

@@ -9,14 +9,13 @@ extern "C"
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context);
B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface(struct b3PluginContext* context);
B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context);
#ifdef __cplusplus

7
examples/SharedMemory/premake4.lua
View File

@@ -20,6 +20,8 @@ language "C++"
myfiles =
{
"b3RobotSimulatorClientAPI_NoDirect.cpp",
"b3RobotSimulatorClientAPI_NoDirect.h",
"IKTrajectoryHelper.cpp",
"IKTrajectoryHelper.h",
"PhysicsClient.cpp",
@@ -58,6 +60,8 @@ myfiles =
"PhysicsServerCommandProcessor.h",
"b3PluginManager.cpp",
"b3PluginManager.h",
"plugins/pdControlPlugin/pdControlPlugin.cpp",
"plugins/pdControlPlugin/pdControlPlugin.h",
"../OpenGLWindow/SimpleCamera.cpp",
"../OpenGLWindow/SimpleCamera.h",
"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
@@ -465,4 +469,7 @@ include "plugins/testPlugin"
include "plugins/vrSyncPlugin"
include "plugins/tinyRendererPlugin"
include "plugins/pdControlPlugin"
include "plugins/collisionFilterPlugin"

5
examples/SharedMemory/tcp/premake4.lua
View File

@@ -91,7 +91,12 @@ myfiles =
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClientC_API.cpp",
"../PhysicsClient.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.h",
"../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../b3RobotSimulatorClientAPI_NoDirect.h",
"../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
"../../TinyRenderer/geometry.cpp",

5
examples/SharedMemory/udp/premake4.lua
View File

@@ -77,6 +77,10 @@ language "C++"
myfiles =
{
"../plugins/pdControlPlugin/pdControlPlugin.cpp",
"../plugins/pdControlPlugin/pdControlPlugin.h",
"../b3RobotSimulatorClientAPI_NoDirect.cpp",
"../b3RobotSimulatorClientAPI_NoDirect.h",
"../IKTrajectoryHelper.cpp",
"../IKTrajectoryHelper.h",
"../SharedMemoryCommands.h",
@@ -85,6 +89,7 @@ myfiles =
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClientC_API.cpp",
"../PhysicsClient.cpp",
"../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",

4
examples/TwoJoint/CMakeLists.txt
View File

@@ -10,6 +10,10 @@ INCLUDE_DIRECTORIES(
SET(RobotSimulator_SRCS
TwoJointMain.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

4
examples/pybullet/CMakeLists.txt
View File

@@ -15,6 +15,10 @@ ENDIF()
SET(pybullet_SRCS
pybullet.c
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

62
examples/pybullet/examples/pdControl.py Normal file
View File

@@ -0,0 +1,62 @@
import pybullet as p
import time
useMaximalCoordinates=False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
for i in range (p.getNumJoints(pole)):
#disable default constraint-based motors
p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
print("joint",i,"=",p.getJointInfo(pole,i))
desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
kdCartId = p.addUserDebugParameter("kpCart",0,300,150)
maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
kdPoleId = p.addUserDebugParameter("kpPole",0,300,100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0,0,0)
useRealTimeSim = True
p.setRealTimeSimulation(useRealTimeSim)
p.setTimeStep(0.001)
while p.isConnected():
if (pd>=0):
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
link = 0
p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosCart, desiredVelCart, kdCart, kpCart, maxForceCart])
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
link = 1
p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosPole, desiredVelPole, kdPole, kpPole, maxForcePole])
if (not useRealTimeSim):
p.stepSimulation()
time.sleep(1./240.)

2
examples/pybullet/examples/renderPlugin.py
View File

@@ -1,7 +1,7 @@
import pybullet as p
p.connect(p.GUI)
plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_tinyRendererPlugin_vs2010_x64_debug.dll","_tinyRendererPlugin")
plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin")
print("plugin=",plugin)
p.loadURDF("r2d2.urdf")

10
examples/pybullet/examples/testPlugin.py Normal file
View File

@@ -0,0 +1,10 @@
import pybullet as p
p.connect(p.GUI)
plugin = p.loadPlugin("D:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin")
print("plugin=",plugin)
p.executePluginCommand(plugin ,"r2d2.urdf", [1,2,3],[50.0,3.3])
while (1):
p.getCameraImage(320,200)
p.stepSimulation()

31
examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf
View File

@@ -0,0 +1,31 @@
<robot name="blob948">
<link name="random_obj_948">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="948.obj" scale="0.015 0.015 0.015"/>
</geometry>
<material name="blockmat">
<color rgba="0.26 0.59 0.61 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="948.obj" scale="0.015 0.015 0.015"/>
</geometry>
</collision>
</link>
</robot>

4
examples/pybullet/premake4.lua
View File

@@ -103,6 +103,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/TinyRenderer/our_gl.cpp",
"../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/SharedMemory/InProcessMemory.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -153,6 +155,8 @@ if not _OPTIONS["no-enet"] then
"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
}
if (_OPTIONS["enable_static_vr_plugin"]) then

38
examples/pybullet/pybullet.c
View File

@@ -399,7 +399,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
}
case eCONNECT_SHARED_MEMORY_SERVER:
{
sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(0);
sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect2(0, key);
break;
}
case eCONNECT_DIRECT:
@@ -680,7 +680,8 @@ static PyObject* pybullet_syncUserData(PyObject* self, PyObject* args, PyObject*
return NULL;
}
Py_RETURN_NONE;
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* keywds)
@@ -756,9 +757,10 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
if (statusType != CMD_REMOVE_USER_DATA_COMPLETED)
{
PyErr_SetString(SpamError, "Error in removeUserData command.");
Py_RETURN_FALSE;
return NULL;
}
Py_RETURN_TRUE;
Py_INCREF(Py_None);
return Py_None;
}
@@ -817,15 +819,17 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) {
Py_RETURN_NONE;
PyErr_SetString(SpamError, "Cannot get user data");
return NULL;
}
switch (value.m_type) {
case USER_DATA_VALUE_TYPE_STRING:
return PyString_FromString((const char *)value.m_data1);
default:
PyErr_SetString(SpamError, "User data value has unknown type");
return NULL;
if (value.m_type != USER_DATA_VALUE_TYPE_STRING)
{
PyErr_SetString(SpamError, "User data value has unknown type");
return NULL;
}
return PyString_FromString((const char *)value.m_data1);
}
static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObject* keywds)
@@ -884,8 +888,9 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId);
if (key == 0 || userDataId == -1) {
PyErr_SetString(SpamError, "Could not get user data info.");
Py_RETURN_NONE;
return NULL;
}
{
PyObject *userDataInfoTuple = PyTuple_New(2);
PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId));
@@ -8311,7 +8316,14 @@ static PyObject* pybullet_executePluginCommand(PyObject* self,
float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i);
b3CustomCommandExecuteAddFloatArgument(command, val);
}
if (seqFloatArgs)
{
Py_DECREF(seqFloatArgs);
}
if (seqIntArgs)
{
Py_DECREF(seqIntArgs);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);

4
setup.py
View File

@@ -49,6 +49,8 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/TinyRenderer/tgaimage.cpp"]\
+["examples/TinyRenderer/our_gl.cpp"]\
+["examples/TinyRenderer/TinyRenderer.cpp"]\
+["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\
+["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\
+["examples/SharedMemory/IKTrajectoryHelper.cpp"]\
+["examples/SharedMemory/InProcessMemory.cpp"]\
+["examples/SharedMemory/PhysicsClient.cpp"]\
@@ -450,7 +452,7 @@ print("-----")
setup(
name = 'pybullet',
version='2.0.3',
version='2.0.5',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

1
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -112,6 +112,7 @@ btMultiBody::btMultiBody(int n_links,
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
m_companionId(-1),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true),

2
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -1664,4 +1664,4 @@ void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodie
m_tmpNumMultiBodyConstraints = 0;
}
}

30
src/LinearMath/btHashMap.h
View File

@@ -23,7 +23,7 @@ subject to the following restrictions:
///very basic hashable string implementation, compatible with btHashMap
struct btHashString
{
std::string m_string;
std::string m_string1;
unsigned int m_hash;
SIMD_FORCE_INLINE unsigned int getHash()const
@@ -33,11 +33,11 @@ struct btHashString
btHashString()
{
m_string="";
m_string1="";
m_hash=0;
}
btHashString(const char* name)
:m_string(name)
:m_string1(name)
{
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
static const unsigned int InitialFNV = 2166136261u;
@@ -46,36 +46,18 @@ struct btHashString
/* Fowler / Noll / Vo (FNV) Hash */
unsigned int hash = InitialFNV;
for(int i = 0; m_string[i]; i++)
for(int i = 0; m_string1.c_str()[i]; i++)
{
hash = hash ^ (m_string[i]); /* xor the low 8 bits */
hash = hash ^ (m_string1.c_str()[i]); /* xor the low 8 bits */
hash = hash * FNVMultiple; /* multiply by the magic number */
}
m_hash = hash;
}
int portableStringCompare(const char* src, const char* dst) const
{
int ret = 0 ;
while( ! (ret = *(const unsigned char *)src - *(const unsigned char *)dst) && *dst)
++src, ++dst;
if ( ret < 0 )
ret = -1 ;
else if ( ret > 0 )
ret = 1 ;
return( ret );
}
bool equals(const btHashString& other) const
{
return (m_string == other.m_string) ||
(0==portableStringCompare(m_string.c_str(),other.m_string.c_str()));
return (m_string1 == other.m_string1);
}
};
const int BT_HASH_NULL=0xffffffff;

123
test/SharedMemory/CMakeLists.txt
View File

@@ -29,66 +29,69 @@ ENDIF()
ADD_EXECUTABLE(Test_PhysicsClientServer
gtestwrap.cpp
../../examples/SharedMemory/PhysicsClient.cpp
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../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/PhysicsLoopBack.cpp
../../examples/SharedMemory/PhysicsLoopBack.h
../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
../../examples/SharedMemory/PhysicsLoopBackC_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/b3Clock.cpp
../../examples/Utils/b3Clock.h
../../examples/Utils/ChromeTraceUtil.cpp
../../examples/Utils/ChromeTraceUtil.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../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/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
../../examples/SharedMemory/IKTrajectoryHelper.cpp
../../examples/SharedMemory/IKTrajectoryHelper.h
../../examples/SharedMemory/PhysicsClient.h
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../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/PhysicsLoopBack.cpp
../../examples/SharedMemory/PhysicsLoopBack.h
../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
../../examples/SharedMemory/PhysicsLoopBackC_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/b3Clock.cpp
../../examples/Utils/b3Clock.h
../../examples/Utils/ChromeTraceUtil.cpp
../../examples/Utils/ChromeTraceUtil.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../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/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
)

16
test/SharedMemory/premake4.lua
View File

@@ -170,6 +170,10 @@ project ("Test_PhysicsServerLoopBack")
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
@@ -186,6 +190,8 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/SharedMemory/PhysicsLoopBack.h",
"../../examples/SharedMemory/PhysicsLoopBackC_API.cpp",
"../../examples/SharedMemory/PhysicsLoopBackC_API.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h",
"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
"../../examples/SharedMemory/PhysicsClientC_API.cpp",
@@ -224,7 +230,7 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
}
if (_OPTIONS["enable_static_plugins"]) then
@@ -256,6 +262,10 @@ end
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
@@ -360,6 +370,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
files {
"test.c",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
"../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",