From aafaa7e33e01282774d6a68175ac38c1a41cf023 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 14 Aug 2017 14:59:41 -0700 Subject: [PATCH 1/5] Expose optional "globalScaling" factor to pybullet.loadURDF and pybullet.loadSDF. This will scale the visual, collision shapes and transform locations. Fix two_cubes.sdf (was lacking collision shape for second cube) --- data/two_cubes.sdf | 23 ++++++++++++-- .../ImportSDFDemo/ImportSDFSetup.cpp | 2 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 10 +++++-- .../ImportURDFDemo/BulletUrdfImporter.h | 3 +- .../ImportURDFDemo/ImportURDFSetup.cpp | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 1 + .../Importers/ImportURDFDemo/UrdfParser.cpp | 30 +++++++++++++------ .../Importers/ImportURDFDemo/UrdfParser.h | 8 ++++- .../InverseDynamicsExample.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 25 ++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 6 ++++ .../PhysicsServerCommandProcessor.cpp | 22 ++++++++++---- .../PhysicsServerCommandProcessor.h | 4 +-- examples/SharedMemory/SharedMemoryCommands.h | 5 +++- examples/pybullet/pybullet.c | 21 ++++++++----- 15 files changed, 130 insertions(+), 34 deletions(-) diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf index 7fda241bd..0a1d0973c 100644 --- a/data/two_cubes.sdf +++ b/data/two_cubes.sdf @@ -142,7 +142,7 @@ - 0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159 + 0.105158 -4.55002 1.499995 -2.89297 -0.988287 -3.14159 1 @@ -155,7 +155,26 @@ 0.166667 - + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 412024c5e..5f360072e 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -187,7 +187,7 @@ void ImportSDFSetup::initPhysics() m_dynamicsWorld->setGravity(gravity); - BulletURDFImporter u2b(m_guiHelper, 0); + BulletURDFImporter u2b(m_guiHelper, 0,1); bool loadOk = u2b.loadSDF(m_fileName); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 86f707a2f..5436e22e5 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -69,6 +69,12 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData { m_pathPrefix[0] = 0; } + + void setGlobalScaling(btScalar scaling) + { + m_urdfParser.setGlobalScaling(scaling); + } + }; void BulletURDFImporter::printTree() @@ -76,10 +82,10 @@ void BulletURDFImporter::printTree() // btAssert(0); } -BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) +BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling) { m_data = new BulletURDFInternalData; - + m_data->setGlobalScaling(globalScaling); m_data->m_guiHelper = helper; m_data->m_customVisualShapesConverter = customConverter; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 0723712dd..2309d6a6f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -15,10 +15,11 @@ class BulletURDFImporter : public URDFImporterInterface public: - BulletURDFImporter(struct GUIHelperInterface* guiHelper, LinkVisualShapesConverter* customConverter); + BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling); virtual ~BulletURDFImporter(); + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 8149f59ec..7e9afeba1 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -194,7 +194,7 @@ void ImportUrdfSetup::initPhysics() } - BulletURDFImporter u2b(m_guiHelper, 0); + BulletURDFImporter u2b(m_guiHelper, 0,1); bool loadOk = u2b.loadURDF(m_fileName); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 961591222..ac96befea 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -173,6 +173,7 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio } +btScalar tmpUrdfScaling=2; void ConvertURDF2BulletInternal( diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 38b10ad17..c4a370c0d 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -6,7 +6,8 @@ UrdfParser::UrdfParser() :m_parseSDF(false), -m_activeSdfModel(-1) +m_activeSdfModel(-1), +m_urdfScaling(1) { m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real } @@ -132,22 +133,24 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err } -bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false) +bool UrdfParser::parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF ) { tr.setIdentity(); + btVector3 vec(0,0,0); if (parseSDF) { - parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger); + parseVector3(vec,std::string(xml->GetText()),logger); } else { const char* xyz_str = xml->Attribute("xyz"); if (xyz_str) { - parseVector3(tr.getOrigin(),std::string(xyz_str),logger); + parseVector3(vec,std::string(xyz_str),logger); } } + tr.setOrigin(vec*m_urdfScaling); if (parseSDF) { @@ -344,7 +347,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* return false; } else { - geom.m_sphereRadius = urdfLexicalCast(shape->Attribute("radius")); + geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); } } else if (type_name == "box") @@ -359,6 +362,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* return false; } parseVector3(geom.m_boxSize,size->GetText(),logger); + geom.m_boxSize *= m_urdfScaling; } else { @@ -369,6 +373,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* } else { parseVector3(geom.m_boxSize,shape->Attribute("size"),logger); + geom.m_boxSize *= m_urdfScaling; } } } @@ -382,8 +387,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* return false; } geom.m_hasFromTo = false; - geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); - geom.m_capsuleHeight = urdfLexicalCast(shape->Attribute("length")); + geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(shape->Attribute("length")); } else if (type_name == "capsule") @@ -396,8 +401,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* return false; } geom.m_hasFromTo = false; - geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); - geom.m_capsuleHeight = urdfLexicalCast(shape->Attribute("length")); + geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(shape->Attribute("length")); } else if (type_name == "mesh") { @@ -438,6 +443,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* } } + geom.m_meshScale *= m_urdfScaling; + if (fn.empty()) { logger->reportError("Mesh filename is empty"); @@ -1031,6 +1038,11 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL joint.m_upperLimit = urdfLexicalCast(upper_str); } + if (joint.m_type == URDFPrismaticJoint) + { + joint.m_lowerLimit *= m_urdfScaling; + joint.m_upperLimit *= m_urdfScaling; + } // Get joint effort limit const char* effort_str = config->Attribute("effort"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 33fa0d49b..7a87ffcc1 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -251,7 +251,8 @@ protected: bool m_parseSDF; int m_activeSdfModel; - + btScalar m_urdfScaling; + bool parseTransform(btTransform& tr, class TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false); bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger); bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger); bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger); @@ -277,6 +278,11 @@ public: { return m_parseSDF; } + void setGlobalScaling(btScalar scaling) + { + m_urdfScaling = scaling; + } + bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase); bool loadSDF(const char* sdfText, ErrorLogger* logger); diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index 323f52dde..adc939cc4 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -156,7 +156,7 @@ void InverseDynamicsExample::initPhysics() - BulletURDFImporter u2b(m_guiHelper,0); + BulletURDFImporter u2b(m_guiHelper,0,1); bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf"); if (loadOk) { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 22ce287a0..5e80652bd 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -223,6 +223,18 @@ int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, return 0; } +int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING; + command->m_urdfArguments.m_globalScaling = globalScaling; + return 0; +} + + + int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -234,6 +246,19 @@ int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, i return 0; } +int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_SDF); + command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING; + command->m_sdfArguments.m_globalScaling = globalScaling; + + return 0; +} + + + int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a0ce40f7b..a600e8067 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -267,6 +267,9 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); +int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); + + b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); @@ -301,6 +304,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); +int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); + + b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2719f2788..70121889b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2284,7 +2284,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS return loadOk; } -bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags) +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling) { btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) @@ -2295,7 +2295,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe m_data->m_sdfRecentLoadedBodies.clear(); - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); bool forceFixedBase = false; bool loadOk =u2b.loadSDF(fileName,forceFixedBase); @@ -2311,7 +2311,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, - bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags) + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling) { m_data->m_sdfRecentLoadedBodies.clear(); *bodyUniqueIdPtr = -1; @@ -2326,7 +2326,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); bool loadOk = u2b.loadURDF(fileName, useFixedBase); @@ -3579,7 +3579,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true; int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA - bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = sdfArgs.m_globalScaling; + } + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling); if (completedOk) { m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); @@ -4016,10 +4021,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true; bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false; int bodyUniqueId; + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = urdfArgs.m_globalScaling; + } //load the actual URDF and send a report: completed or failed bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, initialPos,initialOrn, - useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags); + useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling); if (completedOk && bodyUniqueId>=0) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 83a2129a0..65bb0bb6d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -27,10 +27,10 @@ protected: - bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, - bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags=0); + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling); bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index a5db4cd71..f0834419b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -65,6 +65,7 @@ struct SdfArgs { char m_sdfFileName[MAX_URDF_FILENAME_LENGTH]; int m_useMultiBody; + double m_globalScaling; }; struct FileArgs @@ -79,7 +80,8 @@ enum EnumUrdfArgsUpdateFlags URDF_ARGS_INITIAL_ORIENTATION=4, URDF_ARGS_USE_MULTIBODY=8, URDF_ARGS_USE_FIXED_BASE=16, - URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32 + URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32, + URDF_ARGS_USE_GLOBAL_SCALING =64, }; @@ -91,6 +93,7 @@ struct UrdfArgs int m_useMultiBody; int m_useFixedBase; int m_urdfFlags; + double m_globalScaling; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a4c9549de..2b8acac81 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -929,14 +929,14 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key int physicsClientId = 0; int flags = 0; - static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","physicsClientId", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","globalScaling", "physicsClientId", NULL}; static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL}; static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL}; int bodyUniqueId= -1; const char* urdfFileName = ""; - + double globalScaling = -1; double startPosX = 0.0; double startPosY = 0.0; double startPosZ = 0.0; @@ -976,7 +976,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key double basePos[3]; double baseOrn[4]; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiidi", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &globalScaling, &physicsClientId)) { return NULL; } @@ -1040,7 +1040,10 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key { b3LoadUrdfCommandSetUseFixedBase(command, 1); } - + if (globalScaling>0) + { + b3LoadUrdfCommandSetGlobalScaling(command,globalScaling); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_URDF_LOADING_COMPLETED) @@ -1071,10 +1074,11 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw int statusType; b3SharedMemoryCommandHandle commandHandle; b3PhysicsClientHandle sm = 0; + double globalScaling = -1; int physicsClientId = 0; - static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &sdfFileName, &useMaximalCoordinates, &physicsClientId)) + static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "globalScaling", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|idi", kwlist, &sdfFileName, &useMaximalCoordinates, &globalScaling, &physicsClientId)) { return NULL; } @@ -1090,7 +1094,10 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw { b3LoadSdfCommandSetUseMultiBody(commandHandle,0); } - + if (globalScaling > 0) + { + b3LoadSdfCommandSetUseGlobalScaling(commandHandle,globalScaling); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_SDF_LOADING_COMPLETED) From 60b60ef9fd2517ad6287c7a9da075b0088eb2d35 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 14 Aug 2017 17:02:20 -0700 Subject: [PATCH 2/5] add pybullet.connect(pybullet.GUI_SERVER) option. This allows shared memory connections, acting as a physics server. You can connect using SHARED_MEMORY to this GUI_SERVER. --- .../InProcessExampleBrowser.cpp | 9 ++++---- .../ExampleBrowser/InProcessExampleBrowser.h | 4 ++-- .../PhysicsClientSharedMemory.cpp | 18 ++++++++++----- .../SharedMemoryInProcessPhysicsC_API.cpp | 22 +++++++++---------- .../SharedMemoryInProcessPhysicsC_API.h | 4 ++-- examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/pybullet/pybullet.c | 18 +++++++++++++-- 7 files changed, 49 insertions(+), 27 deletions(-) diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 9c0572626..7b42da5be 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -316,11 +316,12 @@ struct btInProcessExampleBrowserInternalData -btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2) +btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2, bool useInProcessMemory) { btInProcessExampleBrowserInternalData* data = new btInProcessExampleBrowserInternalData; - data->m_sharedMem = new InProcessMemory; + + data->m_sharedMem = useInProcessMemory ? new InProcessMemory : 0; int numThreads = 1; int i; @@ -410,12 +411,12 @@ struct btInProcessExampleBrowserMainThreadInternalData b3Clock m_clock; }; -btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv) +btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv, bool useInProcessMemory) { btInProcessExampleBrowserMainThreadInternalData* data = new btInProcessExampleBrowserMainThreadInternalData; data->m_examples.initExampleEntries(); data->m_exampleBrowser = new DefaultBrowser(&data->m_examples); - data->m_sharedMem = new InProcessMemory; + data->m_sharedMem = useInProcessMemory ? new InProcessMemory : 0; data->m_exampleBrowser->setSharedMemoryInterface(data->m_sharedMem ); bool init; init = data->m_exampleBrowser->init(argc,argv); diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.h b/examples/ExampleBrowser/InProcessExampleBrowser.h index e01d3fd91..32c2c7106 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.h +++ b/examples/ExampleBrowser/InProcessExampleBrowser.h @@ -3,7 +3,7 @@ struct btInProcessExampleBrowserInternalData; -btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2); +btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2, bool useInProcessMemory); bool btIsExampleBrowserTerminated(btInProcessExampleBrowserInternalData* data); @@ -17,7 +17,7 @@ class SharedMemoryInterface* btGetSharedMemoryInterface(btInProcessExampleBrowse struct btInProcessExampleBrowserMainThreadInternalData; -btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv2); +btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv2, bool useInProcessMemory); bool btIsExampleBrowserMainThreadTerminated(btInProcessExampleBrowserMainThreadInternalData* data); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 12585e13b..5e297d28f 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -251,17 +251,23 @@ void PhysicsClientSharedMemory::resetData() m_data->m_userConstraintInfoMap.clear(); } -void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; } +void PhysicsClientSharedMemory::setSharedMemoryKey(int key) +{ + m_data->m_sharedMemoryKey = key; +} void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem) { - if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory) + if (sharedMem) { - delete m_data->m_sharedMemory; - } - m_data->m_ownsSharedMemory = false; - m_data->m_sharedMemory = sharedMem; + if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory) + { + delete m_data->m_sharedMemory; + } + m_data->m_ownsSharedMemory = false; + m_data->m_sharedMemory = sharedMem; + }; } void PhysicsClientSharedMemory::disconnectSharedMemory() { diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 142d42a30..2def80e1c 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -5,7 +5,7 @@ #include "PhysicsClientSharedMemory.h" #include"../ExampleBrowser/InProcessExampleBrowser.h" #include - +#include #include "PhysicsServerExampleBullet2.h" #include "../CommonInterfaces/CommonExampleInterface.h" @@ -19,7 +19,7 @@ class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedM public: - InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[]) + InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[], bool useInProcessMemory) { int newargc = argc+2; char** newargv = (char**)malloc(sizeof(void*)*newargc); @@ -30,7 +30,7 @@ public: char* t1 = (char*)"--start_demo_name=Physics Server"; newargv[argc] = t0; newargv[argc+1] = t1; - m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv); + m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv, useInProcessMemory); SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data); setSharedMemoryInterface(shMem); @@ -83,9 +83,9 @@ public: }; -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]) +b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[], int useInProcessMemory) { - InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv); + InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, useInProcessMemory); cl->setSharedMemoryKey(SHARED_MEMORY_KEY); cl->connect(); return (b3PhysicsClientHandle ) cl; @@ -98,7 +98,7 @@ class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory public: - InProcessPhysicsClientSharedMemory(int argc, char* argv[]) + InProcessPhysicsClientSharedMemory(int argc, char* argv[], bool useInProcessMemory) { int newargc = argc+2; m_newargv = (char**)malloc(sizeof(void*)*newargc); @@ -109,7 +109,7 @@ public: char* t1 = (char*)"--start_demo_name=Physics Server"; m_newargv[argc] = t0; m_newargv[argc+1] = t1; - m_data = btCreateInProcessExampleBrowser(newargc,m_newargv); + m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory); SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data); setSharedMemoryInterface(shMem); } @@ -123,11 +123,11 @@ public: }; -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]) -{ +b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[], int useInProcessMemory) +{ - InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv); - cl->setSharedMemoryKey(SHARED_MEMORY_KEY); + InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, useInProcessMemory); + cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1); cl->connect(); return (b3PhysicsClientHandle ) cl; } diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h index 7ce6e024a..0bad1283a 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h @@ -10,9 +10,9 @@ extern "C" { ///think more about naming. The b3ConnectPhysicsLoopback -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]); +b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[], int useInProcessMemory); -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]); +b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[], int useInProcessMemory); b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 55de32c57..4b61142d4 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -577,6 +577,7 @@ enum eCONNECT_METHOD { eCONNECT_UDP = 4, eCONNECT_TCP = 5, eCONNECT_EXISTING_EXAMPLE_BROWSER=6, + eCONNECT_GUI_SERVER=7, }; enum eURDF_Flags diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2b8acac81..02e3a5d42 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -294,6 +294,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P int method = eCONNECT_GUI; int i; char* options=""; + b3PhysicsClientHandle sm = 0; if (sNumPhysicsClients >= MAX_PHYSICS_CLIENTS) @@ -354,9 +355,21 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P char* argv[2] = {"unused",options}; #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,1); #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,1); +#endif + break; + } + case eCONNECT_GUI_SERVER: + { + int argc = 2; + char* argv[2] = {"unused",options}; + +#ifdef __APPLE__ + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,0); +#else + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,0); #endif break; } @@ -7261,6 +7274,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read + PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read From 9d0f875c5b9647d0598afcbab87b85784a5041f9 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 15 Aug 2017 09:34:44 -0700 Subject: [PATCH 3/5] fix compile issues due to API change (additional argument) --- Extras/InverseDynamics/btMultiBodyFromURDF.hpp | 2 +- .../RobotSimulator/b3RobotSimulatorClientAPI.cpp | 15 +++++++++++++-- test/SharedMemory/test.c | 4 ++-- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp index d627ea985..f3e2a7c05 100644 --- a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -44,7 +44,7 @@ public: void init() { this->createEmptyDynamicsWorld(); m_dynamicsWorld->setGravity(m_gravity); - BulletURDFImporter urdf_importer(&m_nogfx,0); + BulletURDFImporter urdf_importer(&m_nogfx,0,1); URDFImporterInterface &u2b(urdf_importer); bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index a0d2c376c..73c7e30e6 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -129,9 +129,20 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i int argc = 0; char* argv[1] = {0}; #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,1); #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,1); +#endif + break; + } + case eCONNECT_GUI_SERVER: + { + int argc = 0; + char* argv[1] = {0}; +#ifdef __APPLE__ + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,0); +#else + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,0); #endif break; } diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index e475a4705..9dec36e92 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -335,9 +335,9 @@ int main(int argc, char* argv[]) #ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER #ifdef __APPLE__ - b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv); + b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv,1); #else - b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv); + b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv,1); #endif //__APPLE__ #endif From 99beb44625f223e4151d7781ad7057be09809f75 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 15 Aug 2017 10:46:09 -0700 Subject: [PATCH 4/5] fix build issues in previous commit --- .../Importers/ImportURDFDemo/BulletUrdfImporter.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 2309d6a6f..c26d30184 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -15,19 +15,19 @@ class BulletURDFImporter : public URDFImporterInterface public: - BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling); + BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling); virtual ~BulletURDFImporter(); virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); - //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path - virtual bool loadSDF(const char* fileName, bool forceFixedBase = false); - virtual int getNumModels() const; - virtual void activateModel(int modelIndex); - virtual void setBodyUniqueId(int bodyId); - virtual int getBodyUniqueId() const; + //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path + virtual bool loadSDF(const char* fileName, bool forceFixedBase = false); + virtual int getNumModels() const; + virtual void activateModel(int modelIndex); + virtual void setBodyUniqueId(int bodyId); + virtual int getBodyUniqueId() const; const char* getPathPrefix(); void printTree(); //for debugging From 2e0a9877504b6a3edc6bcf14fe38e44d96cadc36 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 15 Aug 2017 14:15:30 -0700 Subject: [PATCH 5/5] pybullet only allow one GUI/GUI_SERVER instance. Fix LINK_FRAME/WORLD_FRAME coordinate issue in pybullet applyExternalForce --- .../PhysicsServerCommandProcessor.cpp | 18 ++++++++++-------- examples/pybullet/pybullet.c | 4 ++-- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 70121889b..9975312da 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6080,28 +6080,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) { - btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - btVector3 positionLocal( + btVector3 tmpPosition( clientCmd.m_externalForceArguments.m_positions[i*3+0], clientCmd.m_externalForceArguments.m_positions[i*3+1], clientCmd.m_externalForceArguments.m_positions[i*3+2]); + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) { - btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; - btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal; + btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpForce : tmpForce; + btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); mb->addBaseForce(forceWorld); mb->addBaseTorque(relPosWorld.cross(forceWorld)); //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); } else { int link = clientCmd.m_externalForceArguments.m_linkIds[i]; - btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal; - btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal; - mb->addLinkForce(link, forceWorld); - mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); + + btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpForce : tmpForce; + btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + mb->addLinkForce(link, forceWorld); + mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); } } diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 02e3a5d42..f1f2b1996 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -338,10 +338,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P int i; for (i = 0; i < MAX_PHYSICS_CLIENTS; i++) { - if (sPhysicsClientsGUI[i] == eCONNECT_GUI) + if ((sPhysicsClientsGUI[i] == eCONNECT_GUI) || (sPhysicsClientsGUI[i] == eCONNECT_GUI_SERVER)) { PyErr_SetString(SpamError, - "Only one local in-process GUI connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY, UDP or TCP instead."); + "Only one local in-process GUI/GUI_SERVER connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY, UDP or TCP instead."); return NULL; } }