From 7577c6d893f3d54fe41e62884fffb511a0c98c45 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 11 Nov 2016 18:07:42 -0800 Subject: [PATCH] add pybullet loadBullet, saveBullet (work-in-progress) and placeholder for loadMJCF. --- .../ImportMJCFDemo/ImportMJCFSetup.cpp | 236 ++++++++++++++++++ .../ImportMJCFDemo/ImportMJCFSetup.h | 8 + examples/SharedMemory/PhysicsClientC_API.cpp | 81 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 6 + .../PhysicsClientSharedMemory.cpp | 20 ++ examples/SharedMemory/PhysicsDirect.cpp | 11 +- .../PhysicsServerCommandProcessor.cpp | 70 ++++++ .../SharedMemory/PhysicsServerExample.cpp | 19 ++ examples/SharedMemory/SharedMemoryCommands.h | 6 + examples/SharedMemory/SharedMemoryPublic.h | 28 ++- examples/pybullet/pybullet.c | 44 +++- 11 files changed, 512 insertions(+), 17 deletions(-) create mode 100644 examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp create mode 100644 examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp new file mode 100644 index 000000000..9375974f2 --- /dev/null +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -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 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 gFileNameArray; + + +#define MAX_NUM_MOTORS 1024 + +struct ImportMJCFInternalData +{ + ImportMJCFInternalData() + :m_numMotors(0), + m_mb(0) + { + for (int i=0;i=numFileNames) + { + count=0; + } + sprintf(m_fileName,"%s",gFileNameArray[count++].c_str()); + } +} + +ImportMJCFSetup::~ImportMJCFSetup() +{ + for (int i=0;isetUpAxis(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;im_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); +} diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h new file mode 100644 index 000000000..52b399426 --- /dev/null +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h @@ -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 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f95ff129a..cc2ac7dcb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 7b4589e86..c22b461e5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 0a3d198eb..03395317c 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -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); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index b54485f66..011204e80 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -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"); } }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6f81102ef..6ab941616 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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 && iloadFile(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"); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index eec1bdbfc..828fab3f4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -74,6 +74,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperCreateRigidBodyGraphicsObject, eGUIHelperRemoveAllGraphicsInstances, eGUIHelperCopyCameraImageData, + eGUIHelperAutogenerateGraphicsObjects, }; #include @@ -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: { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index bdc2dd96e..82925c8f8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1994dc560..47c7e7280 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5956da397..9991ee075 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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; }