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

add pybullet loadBullet, saveBullet (work-in-progress) and placeholder for loadMJCF.

Browse Source
This commit is contained in:
erwin coumans
2016-11-11 18:07:42 -08:00
parent b150edf76b
commit 7577c6d893
11 changed files with 512 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -79,6 +79,87 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
return 0;
}
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_BULLET;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_fileArguments.m_fileName, fileName);
}
else
{
command->m_fileArguments.m_fileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SAVE_BULLET;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_fileArguments.m_fileName, fileName);
}
else
{
command->m_fileArguments.m_fileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_MJCF;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_fileArguments.m_fileName, fileName);
}
else
{
command->m_fileArguments.m_fileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -143,6 +143,11 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
@@ -243,6 +248,7 @@ b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandl
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
///experiments of robots interacting with non-rigid objects (such as btSoftBody)
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);

20
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -703,6 +703,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Load texture failed");
break;
}
case CMD_BULLET_LOADING_COMPLETED:
{
break;
}
case CMD_BULLET_LOADING_FAILED:
{
b3Warning("Load .bullet failed");
break;
}
case CMD_BULLET_SAVING_FAILED:
{
b3Warning("Save .bullet failed");
break;
}
case CMD_MJCF_LOADING_FAILED:
{
b3Warning("Load .mjcf failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);

11
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -662,10 +662,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
break;
}
case CMD_BULLET_LOADING_FAILED:
{
b3Warning("Couldn't load .bullet file");
break;
}
case CMD_BULLET_LOADING_COMPLETED:
{
break;
}
default:
{
// b3Error("Unknown server status type");
b3Warning("Unknown server status type");
}
};

70
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3371,6 +3371,76 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_LOAD_BULLET:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
int numPrefixes = sizeof(prefix) / sizeof(const char*);
char relativeFileName[1024];
FILE* f = 0;
bool found = false;
for (int i = 0; !f && i<numPrefixes; i++)
{
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
f = fopen(relativeFileName, "rb");
if (f)
{
found = true;
break;
}
}
if (f)
{
fclose(f);
}
if (found)
{
bool ok = importer->loadFile(relativeFileName);
if (ok)
{
serverCmd.m_type = CMD_BULLET_LOADING_COMPLETED;
m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
hasStatus = true;
break;
}
}
serverCmd.m_type = CMD_BULLET_LOADING_FAILED;
hasStatus = true;
break;
}
case CMD_SAVE_BULLET:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb");
if (f)
{
btDefaultSerializer* ser = new btDefaultSerializer();
m_data->m_dynamicsWorld->serialize(ser);
fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
fclose(f);
serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED;
delete ser;
}
serverCmd.m_type = CMD_BULLET_SAVING_FAILED;
hasStatus = true;
break;
}
case CMD_LOAD_MJCF:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_MJCF_LOADING_FAILED;
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");

19
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -74,6 +74,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperCreateRigidBodyGraphicsObject,
eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData,
eGUIHelperAutogenerateGraphicsObjects,
};
#include <stdio.h>
@@ -530,8 +531,18 @@ public:
}
btDiscreteDynamicsWorld* m_dynamicsWorld;
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
m_dynamicsWorld = rbWorld;
m_cs->lock();
m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
m_cs->unlock();
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
{
b3Clock::usleep(1000);
}
}
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
@@ -916,6 +927,14 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperAutogenerateGraphicsObjects:
{
m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperIdle:
default:
{

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -67,6 +67,11 @@ struct SdfArgs
int m_useMultiBody;
};
struct FileArgs
{
char m_fileName[MAX_URDF_FILENAME_LENGTH];
};
enum EnumUrdfArgsUpdateFlags
{
URDF_ARGS_FILE_NAME=1,
@@ -509,6 +514,7 @@ struct SharedMemoryCommand
{
struct UrdfArgs m_urdfArguments;
struct SdfArgs m_sdfArguments;
struct FileArgs m_fileArguments;
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;

28
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -11,18 +11,16 @@ enum EnumSharedMemoryClientCommand
CMD_SAVE_BULLET,
CMD_LOAD_MJCF,
CMD_LOAD_BUNNY,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
CMD_CREATE_RIGID_BODY,
CMD_DELETE_RIGID_BODY,
CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED
CMD_INIT_POSE,
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
CMD_CREATE_RIGID_BODY,
CMD_DELETE_RIGID_BODY,
CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
CMD_INIT_POSE,
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
CMD_REQUEST_BODY_INFO,
CMD_REQUEST_INTERNAL_DATA,
CMD_STEP_FORWARD_SIMULATION,
@@ -59,6 +57,12 @@ enum EnumSharedMemoryServerStatus
CMD_SDF_LOADING_FAILED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_LOADING_COMPLETED,
CMD_BULLET_LOADING_FAILED,
CMD_BULLET_SAVING_COMPLETED,
CMD_BULLET_SAVING_FAILED,
CMD_MJCF_LOADING_COMPLETED,
CMD_MJCF_LOADING_FAILED,
CMD_REQUEST_INTERNAL_DATA_COMPLETED,
CMD_REQUEST_INTERNAL_DATA_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,