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)