diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 514ac970d..84b5fc7db 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -86,6 +86,7 @@ public: b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; args.m_fileType = B3_SDF_FILE; + args.m_useMultiBody = true; b3RobotSimLoadFileResults results; if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) @@ -131,11 +132,10 @@ public: { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.sdf"; + args.m_fileName = "cube_small.urdf"; args.m_startPosition.setValue(0,0,.107); args.m_startOrientation.setEulerZYX(0,0,0); args.m_useMultiBody = false; - args.m_fileType = 2; m_robotSim.loadFile(args,results); } if (1) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 634ba3cd2..15cc08a21 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -703,6 +703,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str()); + b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); statusType = b3GetStatusType(statusHandle); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e60a4f5d2..98d6d2e19 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -66,6 +66,17 @@ int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, return 0; } +int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_SDF); + command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY; + command->m_sdfArguments.m_useMultiBody = useMultiBody; + + 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 6f7b13647..545b70b9e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -96,6 +96,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 b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f1436516b..5e2970b83 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -711,7 +711,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) } -bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes) +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody) { btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) @@ -768,7 +768,6 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe MyMultiBodyCreator creation(m_data->m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - bool useMultiBody = true; ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); @@ -1244,8 +1243,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? sdfArgs.m_useMultiBody : true; - bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody); if (completedOk) { //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 692bc2dd9..6526fe030 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -21,7 +21,7 @@ protected: - bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes); + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ac5781639..f7a9812ca 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -64,6 +64,7 @@ enum EnumSdfArgsUpdateFlags struct SdfArgs { char m_sdfFileName[MAX_URDF_FILENAME_LENGTH]; + int m_useMultiBody; }; enum EnumUrdfArgsUpdateFlags