From 49119169374645fe6e37c812cc784046efee76d2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 27 Mar 2017 08:30:20 -0700 Subject: [PATCH] Expose 'flags' option for loadURDF, to allow customization of the URDF loading process while maintaining backward compatibility. For example: URDF_USE_INERTIA_FROM_FILE flag. By default, URDF2Bullet will re-compute the inertia tensor based on mass and volume, because most URDF files have bogus Inertia values. --- .../RobotSimulator/b3RobotSimulatorClientAPI.cpp | 2 ++ .../RobotSimulator/b3RobotSimulatorClientAPI.h | 4 +++- examples/SharedMemory/PhysicsClientC_API.cpp | 13 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsServerCommandProcessor.cpp | 14 +++++++++----- .../SharedMemory/PhysicsServerCommandProcessor.h | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/SharedMemory/SharedMemoryPublic.h | 5 +++++ examples/pybullet/pybullet.c | 14 ++++++++------ 9 files changed, 44 insertions(+), 13 deletions(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 64b062964..d29b7407f 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -260,6 +260,8 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc //setting the initial position, orientation and other arguments are optional + b3LoadUrdfCommandSetFlags(command,args.m_flags); + b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index bac77009a..b7d9991e5 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -15,12 +15,14 @@ struct b3RobotSimulatorLoadUrdfFileArgs b3Quaternion m_startOrientation; bool m_forceOverrideFixedBase; bool m_useMultiBody; + int m_flags; b3RobotSimulatorLoadUrdfFileArgs() : m_startPosition(b3MakeVector3(0, 0, 0)), m_startOrientation(b3Quaternion(0, 0, 0, 1)), m_forceOverrideFixedBase(false), - m_useMultiBody(true) + m_useMultiBody(true), + m_flags(0) { } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index bf2c95dad..7615782ba 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -238,6 +238,19 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, return -1; } +int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + if (command && (command->m_type == CMD_LOAD_URDF)) + { + command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS; + command->m_urdfArguments.m_urdfFlags = flags; + } + return 0; +} + int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ff553680c..35071ae34 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -217,6 +217,7 @@ int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 614dc8da5..84371ae89 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1539,7 +1539,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) + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags) { BT_PROFILE("loadURDF"); btAssert(m_data->m_dynamicsWorld); @@ -1597,7 +1597,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_data->m_guiHelper); - ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); + ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); for (int i=0;im_sphereId = bodyId; InteralBodyData* parentBody = m_data->getHandle(bodyId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 7c1732002..180db5c58 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -35,7 +35,7 @@ protected: bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, - bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags=0); 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 fdc1aa709..096349762 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -79,6 +79,7 @@ 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 }; @@ -89,6 +90,7 @@ struct UrdfArgs double m_initialOrientation[4]; int m_useMultiBody; int m_useFixedBase; + int m_urdfFlags; }; struct MjcfArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 6587747d4..9692014d2 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -447,4 +447,9 @@ enum eCONNECT_METHOD { eCONNECT_TCP = 5, }; +enum eURDF_Flags +{ + URDF_USE_INERTIA_FROM_FILE=2,//sync with URDF2Bullet.h 'ConvertURDFFlags' +}; + #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ddf444d6e..c2e44185a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -693,8 +693,9 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; + int flags = 0; - static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "physicsClientId", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","physicsClientId", NULL}; static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL}; static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL}; @@ -741,7 +742,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key double basePos[3]; double baseOrn[4]; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &physicsClientId)) { return NULL; } @@ -790,6 +791,8 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); + b3LoadUrdfCommandSetFlags(command,flags); + // setting the initial position, orientation and other arguments are // optional b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); @@ -3916,15 +3919,13 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff); b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); - b3RequestCameraImageSelectRenderer(command, renderer); + b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle statusHandle; int statusType; - // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CAMERA_IMAGE_COMPLETED) @@ -4349,7 +4350,6 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) b3SharedMemoryStatusHandle statusHandle; int statusType; - // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -5422,6 +5422,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); + PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); + PyModule_AddIntConstant(m, "B3G_F1", B3G_F1); PyModule_AddIntConstant(m, "B3G_F2", B3G_F2); PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);