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

Support loading Urdf as btRigidBody with RobotSimAPI. Loading Sdf as btRigidBody is work in progress.

Browse Source
This commit is contained in:
yunfeibai
2016-08-16 17:56:30 -07:00
parent 4bc31394a0
commit 591f922d97
7 changed files with 20 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -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)

1
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -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);

11
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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,

6
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -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);

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -64,6 +64,7 @@ enum EnumSdfArgsUpdateFlags
struct SdfArgs
{
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
int m_useMultiBody;
};
enum EnumUrdfArgsUpdateFlags