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

236
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp Normal file
View File

@@ -0,0 +1,236 @@
#include "ImportMJCFSetup.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
//#define TEST_MULTIBODY_SERIALIZATION 1
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "Bullet3Common/b3FileUtils.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
class ImportMJCFSetup : public CommonMultiBodyBase
{
char m_fileName[1024];
struct ImportMJCFInternalData* m_data;
bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory;
btScalar m_grav;
int m_upAxis;
public:
ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportMJCFSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void setFileName(const char* mjcfFileName);
virtual void resetCamera()
{
float dist = 3.5;
float pitch = -136;
float yaw = 28;
float targetPos[3]={0.47,0,-0.64};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
};
btAlignedObjectArray<std::string> gFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportMJCFInternalData
{
ImportMJCFInternalData()
:m_numMotors(0),
m_mb(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
btMultiBody* m_mb;
btRigidBody* m_rb;
};
ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper),
m_grav(0),
m_upAxis(2)
{
m_data = new ImportMJCFInternalData;
if (option==1)
{
m_useMultiBody = true;
} else
{
m_useMultiBody = false;
}
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
{
gFileNameArray.clear();
//load additional MJCF file names from file
FILE* f = fopen("mjcf_files.txt","r");
if (f)
{
int result;
//warning: we don't avoid string buffer overflow in this basic example in fscanf
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("mjcf_files.txt entry %s",fileName);
if (result==1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
fclose(f);
}
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("quadruped/quadruped.mjcf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
{
count=0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
}
}
ImportMJCFSetup::~ImportMJCFSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
}
static btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
static btVector3 selectColor()
{
static int curColor = 0;
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
return color;
}
void ImportMJCFSetup::setFileName(const char* mjcfFileName)
{
memcpy(m_fileName,mjcfFileName,strlen(mjcfFileName)+1);
}
void ImportMJCFSetup::initPhysics()
{
m_guiHelper->setUpAxis(m_upAxis);
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
if (m_guiHelper->getParameterInterface())
{
SliderParams slider("Gravity", &m_grav);
slider.m_minVal = -10;
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
void ImportMJCFSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
btVector3 gravity(0, 0, 0);
gravity[m_upAxis] = m_grav;
m_dynamicsWorld->setGravity(gravity);
for (int i=0;i<m_data->m_numMotors;i++)
{
if (m_data->m_jointMotors[i])
{
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
}
if (m_data->m_generic6DofJointMotors[i])
{
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
//jointInfo->
}
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
}
}
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options)
{
return new ImportMJCFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
}

8
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef IMPORT_MJCF_SETUP_H
#define IMPORT_MJCF_SETUP_H
class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options);
#endif //IMPORT_MJCF_SETUP_H

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,

44
examples/pybullet/pybullet.c
View File

@@ -204,21 +204,38 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
{
int size = PySequence_Size(args);
const char* bulletFileName = "";
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
return NULL;
}
if (size == 1) {
if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
}
return NULL;
command = b3LoadBulletCommandInit(sm, bulletFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_BULLET_LOADING_COMPLETED)
{
PyErr_SetString(SpamError, "Couldn't load .bullet file.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
{
int size = PySequence_Size(args);
const char* bulletFileName = "";
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -227,7 +244,15 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
if (size == 1) {
if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
}
return NULL;
command = b3SaveBulletCommandInit(sm, bulletFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
if (statusHandle != CMD_BULLET_SAVING_COMPLETED)
{
PyErr_SetString(SpamError, "Couldn't save .bullet file.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
@@ -236,6 +261,9 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
{
int size = PySequence_Size(args);
const char* mjcfjFileName = "";
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -244,7 +272,15 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
if (size == 1) {
if (!PyArg_ParseTuple(args, "s", &mjcfjFileName)) return NULL;
}
return NULL;
command = b3LoadMJCFCommandInit(sm, mjcfjFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
if (statusHandle != CMD_MJCF_LOADING_COMPLETED)
{
PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}