From 35fc8306fd3d7828da334a96f54b80aaaa82bac4 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 11 Nov 2016 14:44:50 -0800 Subject: [PATCH 1/5] pybullet load/save .bullet files --- examples/SharedMemory/SharedMemoryPublic.h | 3 ++ examples/pybullet/pybullet.c | 60 +++++++++++++++++++++- 2 files changed, 61 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7f8179b6c..1994dc560 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,6 +7,9 @@ enum EnumSharedMemoryClientCommand { CMD_LOAD_SDF, CMD_LOAD_URDF, + CMD_LOAD_BULLET, + CMD_SAVE_BULLET, + CMD_LOAD_MJCF, CMD_LOAD_BUNNY, CMD_SEND_BULLET_DATA_STREAM, CMD_CREATE_BOX_COLLISION_SHAPE, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a38d427fd..a691b1dc2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -197,12 +197,59 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) { PyErr_SetString(SpamError, "Cannot execute saveWorld command."); return NULL; +} + + +static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + const char* bulletFileName = ""; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL; + } + return NULL; +} + +static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + const char* bulletFileName = ""; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL; + } + return NULL; +} -// Load a URDF file indicating the links and joints of an object +static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + const char* mjcfjFileName = ""; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (size == 1) { + if (!PyArg_ParseTuple(args, "s", &mjcfjFileName)) return NULL; + } + return NULL; +} + + + +// Load a robot from a URDF file (universal robot description format) // function can be called without arguments and will default // to position (0,0,1) with orientation(0,0,0,1) // els(x,y,z) or @@ -2515,8 +2562,17 @@ static PyMethodDef SpamMethods[] = { {"loadSDF", pybullet_loadSDF, METH_VARARGS, "Load multibodies from an SDF file."}, + { "loadBullet", pybullet_loadBullet, METH_VARARGS, + "Restore the full state of the world from a .bullet file." }, + + { "saveBullet", pybullet_saveBullet, METH_VARARGS, + "Save the full state of the world to a .bullet file." }, + + { "loadMJCF", pybullet_loadMJCF, METH_VARARGS, + "Load multibodies from an MJCF file." }, + {"saveWorld", pybullet_saveWorld, METH_VARARGS, - "Save an approximate Python file to reproduce the current state of the world: saveWorld" + "Save a approximate Python file to reproduce the current state of the world: saveWorld" "(filename). (very preliminary and approximately)"}, {"getNumBodies", pybullet_getNumBodies, METH_VARARGS, From 7577c6d893f3d54fe41e62884fffb511a0c98c45 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 11 Nov 2016 18:07:42 -0800 Subject: [PATCH 2/5] 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; } From 9bad64fed9c332da00e76891cf7d4a319822bc85 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 12 Nov 2016 11:59:40 -0800 Subject: [PATCH 3/5] allow obsolete OpenGL2 drawer to render with z-up axis. --- data/kiva_shelf/0_Bullet3Demo.txt | 7 ------ examples/ExampleBrowser/GL_ShapeDrawer.cpp | 22 +++++++++++++------ examples/ExampleBrowser/GL_ShapeDrawer.h | 4 ++-- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 2 +- .../ImportMJCFDemo/ImportMJCFSetup.cpp | 14 ++++++------ examples/OpenGLWindow/SimpleOpenGL2App.cpp | 8 ++++++- .../OpenGLWindow/SimpleOpenGL2Renderer.cpp | 1 + 7 files changed, 33 insertions(+), 25 deletions(-) delete mode 100644 data/kiva_shelf/0_Bullet3Demo.txt diff --git a/data/kiva_shelf/0_Bullet3Demo.txt b/data/kiva_shelf/0_Bullet3Demo.txt deleted file mode 100644 index eb396d29f..000000000 --- a/data/kiva_shelf/0_Bullet3Demo.txt +++ /dev/null @@ -1,7 +0,0 @@ ---start_demo_name=R2D2 Grasp ---mouse_move_multiplier=0.400000 ---mouse_wheel_multiplier=0.010000 ---background_color_red= 0.900000 ---background_color_green= 0.900000 ---background_color_blue= 1.000000 ---fixed_timestep= 0.000000 diff --git a/examples/ExampleBrowser/GL_ShapeDrawer.cpp b/examples/ExampleBrowser/GL_ShapeDrawer.cpp index 99b6421d6..2b730af5b 100644 --- a/examples/ExampleBrowser/GL_ShapeDrawer.cpp +++ b/examples/ExampleBrowser/GL_ShapeDrawer.cpp @@ -782,7 +782,7 @@ GL_ShapeDrawer::~GL_ShapeDrawer() } } -void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWorld, int pass) +void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWorld, int pass, int cameraUpAxis) { btAssert(dynamicsWorld); @@ -849,7 +849,12 @@ void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWo //if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe)) int debugMode = 0;//getDebugMode() //btVector3 m_sundirection(-1,-1,-1); + btVector3 m_sundirection(btVector3(1,-2,1)*1000); + if (cameraUpAxis==2) + { + m_sundirection = btVector3(1,1,-2)*1000; + } switch(pass) { @@ -861,9 +866,12 @@ void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWo } -void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, bool useShadows) +//this GL_ShapeDrawer will be removed, in the meanwhile directly access this global 'useShadoMaps' +extern bool useShadowMap; +void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, bool useShadows1, int cameraUpAxis) { + bool useShadows = useShadowMap; GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) }; GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0) }; GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0 )}; @@ -897,7 +905,7 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo { glClear(GL_STENCIL_BUFFER_BIT); glEnable(GL_CULL_FACE); - drawSceneInternal(dynamicsWorld,0); + drawSceneInternal(dynamicsWorld,0, cameraUpAxis); glDisable(GL_LIGHTING); glDepthMask(GL_FALSE); @@ -907,10 +915,10 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL); glFrontFace(GL_CCW); glStencilOp(GL_KEEP,GL_KEEP,GL_INCR); - drawSceneInternal(dynamicsWorld,1); + drawSceneInternal(dynamicsWorld,1,cameraUpAxis); glFrontFace(GL_CW); glStencilOp(GL_KEEP,GL_KEEP,GL_DECR); - drawSceneInternal(dynamicsWorld,1); + drawSceneInternal(dynamicsWorld,1,cameraUpAxis); glFrontFace(GL_CCW); glPolygonMode(GL_FRONT,GL_FILL); @@ -929,7 +937,7 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL ); glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP ); glDisable(GL_LIGHTING); - drawSceneInternal(dynamicsWorld,2); + drawSceneInternal(dynamicsWorld,2,cameraUpAxis); glEnable(GL_LIGHTING); glDepthFunc(GL_LESS); glDisable(GL_STENCIL_TEST); @@ -938,6 +946,6 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo else { glDisable(GL_CULL_FACE); - drawSceneInternal(dynamicsWorld,0); + drawSceneInternal(dynamicsWorld,0,cameraUpAxis); } } \ No newline at end of file diff --git a/examples/ExampleBrowser/GL_ShapeDrawer.h b/examples/ExampleBrowser/GL_ShapeDrawer.h index 4d9ce701a..e004420c4 100644 --- a/examples/ExampleBrowser/GL_ShapeDrawer.h +++ b/examples/ExampleBrowser/GL_ShapeDrawer.h @@ -44,7 +44,7 @@ protected: ShapeCache* cache(btConvexShape*); - virtual void drawSceneInternal(const btDiscreteDynamicsWorld* world, int pass); + virtual void drawSceneInternal(const btDiscreteDynamicsWorld* world, int pass, int cameraUpAxis); public: GL_ShapeDrawer(); @@ -53,7 +53,7 @@ public: - virtual void drawScene(const btDiscreteDynamicsWorld* world, bool useShadows); + virtual void drawScene(const btDiscreteDynamicsWorld* world, bool useShadows, int cameraUpAxis); ///drawOpenGL might allocate temporary memoty, stores pointer in shape userpointer virtual void drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 4a4277b2d..0194b4f4e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -338,7 +338,7 @@ void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld) if (m_data->m_gl2ShapeDrawer && rbWorld) { m_data->m_gl2ShapeDrawer->enableTexture(true); - m_data->m_gl2ShapeDrawer->drawScene(rbWorld,true); + m_data->m_gl2ShapeDrawer->drawScene(rbWorld,true, m_data->m_glApp->getUpAxis()); } } void OpenGLGuiHelper::createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld) diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index 9375974f2..f4259f77e 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -44,7 +44,7 @@ public: }; -btAlignedObjectArray gFileNameArray; +static btAlignedObjectArray gMCFJFileNameArray; #define MAX_NUM_MOTORS 1024 @@ -94,7 +94,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, setFileName(fileName); } else { - gFileNameArray.clear(); + gMCFJFileNameArray.clear(); @@ -112,26 +112,26 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, b3Printf("mjcf_files.txt entry %s",fileName); if (result==1) { - gFileNameArray.push_back(fileName); + gMCFJFileNameArray.push_back(fileName); } } while (result==1); fclose(f); } - if (gFileNameArray.size()==0) + if (gMCFJFileNameArray.size()==0) { - gFileNameArray.push_back("quadruped/quadruped.mjcf"); + gMCFJFileNameArray.push_back("quadruped/quadruped.mjcf"); } - int numFileNames = gFileNameArray.size(); + int numFileNames = gMCFJFileNameArray.size(); if (count>=numFileNames) { count=0; } - sprintf(m_fileName,"%s",gFileNameArray[count++].c_str()); + sprintf(m_fileName,"%s",gMCFJFileNameArray[count++].c_str()); } } diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.cpp b/examples/OpenGLWindow/SimpleOpenGL2App.cpp index a898dd602..710b9fdab 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2App.cpp @@ -70,6 +70,11 @@ struct SimpleOpenGL2AppInternalData { GLuint m_fontTextureId; GLuint m_largeFontTextureId; + int m_upAxis; + SimpleOpenGL2AppInternalData() + :m_upAxis(1) + { + } }; static GLuint BindFont2(const CTexFont *_Font) @@ -268,10 +273,11 @@ void SimpleOpenGL2App::drawGrid(DrawGridData data) } void SimpleOpenGL2App::setUpAxis(int axis) { + this->m_data->m_upAxis = axis; } int SimpleOpenGL2App::getUpAxis() const { - return 1; + return this->m_data->m_upAxis; } void SimpleOpenGL2App::swapBuffer() diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 050ec447e..d69b63d54 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp @@ -33,6 +33,7 @@ void SimpleOpenGL2Renderer::updateCamera(int upAxis) float projection[16]; float view[16]; m_camera.setAspectRatio((float)m_width/(float)m_height); + m_camera.setCameraUpAxis(upAxis); m_camera.update(); m_camera.getCameraProjectionMatrix(projection); m_camera.getCameraViewMatrix(view); From d49e3d787a7e5a4a84c427c253b1b9cdf9788c83 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 12 Nov 2016 12:24:20 -0800 Subject: [PATCH 4/5] fix rolling/spinning friction in raytest demo. --- examples/Raycast/RaytestDemo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Raycast/RaytestDemo.cpp b/examples/Raycast/RaytestDemo.cpp index fa07a9942..5bf458d7e 100644 --- a/examples/Raycast/RaytestDemo.cpp +++ b/examples/Raycast/RaytestDemo.cpp @@ -200,7 +200,6 @@ void RaytestDemo::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setRollingFriction(1); body->setFriction(1); //add the body to the dynamics world m_dynamicsWorld->addRigidBody(body); @@ -269,6 +268,7 @@ void RaytestDemo::initPhysics() rbInfo.m_startWorldTransform = startTransform; btRigidBody* body = new btRigidBody(rbInfo); body->setRollingFriction(0.03); + body->setSpinningFriction(0.03); body->setFriction(1); body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(),btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); From c521d816c6e3c7574d330df40e1f3bbffa9eded9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 14 Nov 2016 07:39:34 -0800 Subject: [PATCH 5/5] add user debug line/text features in pybullet + shared memory API: addUserDebugLine, addUserDebugText removeUserDebugItem removeAllUserDebugItems --- data/multibody.bullet | Bin 16496 -> 14712 bytes .../CommonGUIHelperInterface.h | 23 +- examples/ExampleBrowser/OpenGLGuiHelper.h | 18 ++ examples/RoboticsLearning/b3RobotSimAPI.cpp | 15 + examples/SharedMemory/PhysicsClientC_API.cpp | 103 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 12 +- .../PhysicsClientSharedMemory.cpp | 9 + .../PhysicsServerCommandProcessor.cpp | 94 ++++++- .../SharedMemory/PhysicsServerExample.cpp | 220 ++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 36 +++ examples/SharedMemory/SharedMemoryPublic.h | 5 + .../TinyRendererVisualShapeConverter.cpp | 14 +- .../TinyRendererVisualShapeConverter.h | 2 +- examples/SharedMemory/udp/main.cpp | 14 +- examples/SoftDemo/SoftDemo.cpp | 4 +- .../main_tinyrenderer_single_example.cpp | 14 + examples/pybullet/pybullet.c | 261 +++++++++++++++++- 17 files changed, 809 insertions(+), 35 deletions(-) diff --git a/data/multibody.bullet b/data/multibody.bullet index c96db53b5cb53cb6128258c7b04bb07a1ebec1dd..90aa451573fd7c8611a929f7947ef633b70b227f 100644 GIT binary patch delta 2243 zcmbtVU1(HS5T2XdCi|17YqG&@Y}3`oQj3OINt=-UcTHDK&6>2`2HO%_Nf#;_7wLc5 zTLf7}B$wNVY@i^dkHxx(+6Om{FCt|PfkM~5<)z@01S!P_!G3ejJ^Lp@iRUtV&iv1r zGc&tpXb&<+V(*TQ?s;c=$LFsP4#ZQV@iY-N*{~hE2AI!3A0{%w^Z9(}OfDAy3F|_X zX}3$GAqJ+n$}bmhmr6lC&*vwhb1U}y{kjwGF?>}c{=>b@uMQejutz`*7uGr1FS05Wl@f!Sh*-1pfl_M z)D~+i@K&?Xm3?wOLZW7=Y;leHBj~E%B=aftt<#RpeQZMk(P!?DKCE#}qAMdWj3|HB zc4x27)q#ADcPl*=M->NqD()&T45PIJ3h=zPGopz^G&u;XA{W>@0kt_|hkk$+cl$TE z%L{?!Y)eJSX`OIyG_Sj?^X{G(!5lscNoCQ6GlQ^bZD{Yb&Pfx)AXcQ@me$re$rOH} zXZcV`gS-w8C-fMGRH?{Z#y4W$Jr>*NL#S;yGrS6@job;`X%;%O_w|ABKl*_ERzx_5 znFfBtQV#295F7^W0K^CaUTg@6@kJFNmmrTMIU`*>nZz>I4pz&VBjtG|TeJFLELrpT zBFM)c-_zc^4RVG`f$H(b6o@BSUxAkwJQ0qC*Lo7b^~8zLxCX}93gS(E{ec*Nj67`u zw(>Ai2~eHim;kpG^>mp_`Z_xN8}GeJh5WtKyBK}196Zo>4Hcl%Tq;#O^~=^!K2;9# zIA6IL3tj2`PL$_G-0k1Bw$K+S*CXI_lV--==nL_sKTVa3Bjja;MJR}JVgtwxoGJK! z70}LV@dMGFfj(}W%dd1S`?DVECl5ZqfPR{y2pMqLPf1XwXcoVN_`H;^G{Yq(6>r%5r@bFhCEE$y6?$Vr=(^ko$-O)b=?s09@(%#37|Eih8=oz9AwHO#QV4XGr2@WqgbCCrCC^dOYn zmn?`yMF?SnKIqeiJrye=11Wq^g1~xN4?$33=iKjpyZ4$nMYD*`<=Z*up5O1D^PO+s zu4H?Kc>URMxUr?Z?P^u^;ZSQhGzI`RT>EhexDILXK|&HCD=~gjD%PN1f6t@<0hv)* z>COPb^Bm?Hh~?SX!xZA=z`(da=YTo>tn*Qs6r*+fj=7NVU?!AQuNt)PDlbbTsXHhb&yRjFK z&Gx9w{1qVeQU}V8$Y66YnnVG~BDm;1sEzg=B}NWI$7f@np{W#R=oL-O7M89z&Yk`t zoIgHo$_#uH{N36T&mVtn{?U5bGROHgu+9qz#8PJ&-DT8$rI~P-E71>-(rOGyUuz~i zTItMAQ^VHlQd0&fQ;O1z!+IDyR7Bp7iyx;=rkya-GQ6vd!HH7S@OH@h-J}n5?#5!hJHIt1 zc`B#I)j0PTM~xJC9kgFGxTHqx4Y>_)pB!^z z&Kp=aZ9((G+Up(~uiX;EiC-|wX^!QjUriMM%!yzmpq_VbW!dyNeZ{V+2Q@hnhA&hv zI7^v0!^B6;VwXi6M{A3?2tAbI;)SHbk~!Yfm<(U|E)%Xpet|FINw48_{mB_LcB9>1 zlb=GQIVir^<$KAGQf+Hu4-qLY;1~s(0$0ROSH6n-p7L0C*vJ{fzJt*^#E1Dm6k>S) z5^cbY-Yx+7TAKP^eIj}e&}ZT$ql?B|GWwHd19R8Ch#6>#`oeCbT@sIe6Uik{#PzItZ zRBns6Kmx~H03Q#?;J`=3<%8`|1J!t5!{9}Vp3Nu^!HggZx8WR&K?h`oVY{f=0!qkJ zHz>k1^a(w}TlfJPK@qxb3EQY61D%2bedwbdm_type) { + case CMD_BULLET_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED: { int i,maxBodies; @@ -1062,6 +1063,107 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l } + +/// Add/remove user-specific debug lines and debug text messages +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_HAS_LINE; //USER_DEBUG_HAS_TEXT + + command->m_userDebugDrawArgs.m_debugLineFromXYZ[0] = fromXYZ[0]; + command->m_userDebugDrawArgs.m_debugLineFromXYZ[1] = fromXYZ[1]; + command->m_userDebugDrawArgs.m_debugLineFromXYZ[2] = fromXYZ[2]; + + command->m_userDebugDrawArgs.m_debugLineToXYZ[0] = toXYZ[0]; + command->m_userDebugDrawArgs.m_debugLineToXYZ[1] = toXYZ[1]; + command->m_userDebugDrawArgs.m_debugLineToXYZ[2] = toXYZ[2]; + + command->m_userDebugDrawArgs.m_debugLineColorRGB[0] = colorRGB[0]; + command->m_userDebugDrawArgs.m_debugLineColorRGB[1] = colorRGB[1]; + command->m_userDebugDrawArgs.m_debugLineColorRGB[2] = colorRGB[2]; + + command->m_userDebugDrawArgs.m_lineWidth = lineWidth; + command->m_userDebugDrawArgs.m_lifeTime = lifeTime; + + return (b3SharedMemoryCommandHandle) command; +} + +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime) +{ + + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_HAS_TEXT; + + int len = strlen(txt); + if (lenm_userDebugDrawArgs.m_text,txt); + } else + { + command->m_userDebugDrawArgs.m_text[0] = 0; + } + command->m_userDebugDrawArgs.m_textPositionXYZ[0] = positionXYZ[0]; + command->m_userDebugDrawArgs.m_textPositionXYZ[1] = positionXYZ[1]; + command->m_userDebugDrawArgs.m_textPositionXYZ[2] = positionXYZ[2]; + + command->m_userDebugDrawArgs.m_textColorRGB[0] = colorRGB[0]; + command->m_userDebugDrawArgs.m_textColorRGB[1] = colorRGB[1]; + command->m_userDebugDrawArgs.m_textColorRGB[2] = colorRGB[2]; + + command->m_userDebugDrawArgs.m_textSize = textSize; + + command->m_userDebugDrawArgs.m_lifeTime = lifeTime; + + return (b3SharedMemoryCommandHandle) command; +} + +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM; + command->m_userDebugDrawArgs.m_removeItemUniqueId = debugItemUniqueId; + return (b3SharedMemoryCommandHandle) command; + +} + +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_REMOVE_ALL; + return (b3SharedMemoryCommandHandle) command; +} + +int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED); + if (status->m_type != CMD_USER_DEBUG_DRAW_COMPLETED) + return -1; + + return status->m_userDebugDrawArgs.m_debugItemUniqueId; +} + + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) { @@ -1627,6 +1729,7 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle return (b3SharedMemoryCommandHandle)command; } + int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c22b461e5..e82b6824b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -74,14 +74,22 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); -///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet +///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); -///Get the pointers to the debug line information, after b3InitRequestDebugLinesCommand returns +///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns ///status CMD_DEBUG_LINES_COMPLETED void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines); +/// Add/remove user-specific debug lines and debug text messages +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime); +b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); +b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); +///All debug items unique Ids are positive: a negative unique Id means failure. +int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 03395317c..e2b072b44 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -722,6 +722,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Load .mjcf failed"); break; } + case CMD_USER_DEBUG_DRAW_COMPLETED: + { + break; + } + case CMD_USER_DEBUG_DRAW_FAILED: + { + b3Warning("User debug draw failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6ab941616..b51d5c736 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3363,9 +3363,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); + int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); - serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + if (uid>=0) + { + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + } else + { + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + } hasStatus = true; break; @@ -3403,6 +3409,34 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool ok = importer->loadFile(relativeFileName); if (ok) { + + + int numRb = importer->getNumRigidBodies(); + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + + for( int i=0;igetRigidBodyByIndex(i); + + if (colObj) + { + btRigidBody* rb = btRigidBody::upcast(colObj); + if (rb) + { + int bodyUniqueId = m_data->allocHandle(); + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + colObj->setUserIndex2(bodyUniqueId); + bodyHandle->m_rigidBody = rb; + + if (serverStatusOut.m_sdfLoadedArgs.m_numBodiesm_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); hasStatus = true; @@ -3441,6 +3475,62 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_USER_DEBUG_DRAW: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; + hasStatus = true; + + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) + { + int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, + clientCmd.m_userDebugDrawArgs.m_textColorRGB, + clientCmd.m_userDebugDrawArgs.m_textSize, + clientCmd.m_userDebugDrawArgs.m_lifeTime); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) + { + int uid = m_data->m_guiHelper->addUserDebugLine( + clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, + clientCmd.m_userDebugDrawArgs.m_lineWidth, + clientCmd.m_userDebugDrawArgs.m_lifeTime); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) + { + m_data->m_guiHelper->removeAllUserDebugItems(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) + { + m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_removeItemUniqueId); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + + break; + } + + default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 828fab3f4..4e3ccf253 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -75,6 +75,10 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveAllGraphicsInstances, eGUIHelperCopyCameraImageData, eGUIHelperAutogenerateGraphicsObjects, + eGUIUserDebugAddText, + eGUIUserDebugAddLine, + eGUIUserDebugRemoveItem, + eGUIUserDebugRemoveAllItems, }; #include @@ -271,17 +275,43 @@ void* MotionlsMemoryFunc() +struct UserDebugDrawLine +{ + double m_debugLineFromXYZ[3]; + double m_debugLineToXYZ[3]; + double m_debugLineColorRGB[3]; + double m_lineWidth; + + double m_lifeTime; + int m_itemUniqueId; +}; + +struct UserDebugText +{ + char m_text[1024]; + double m_textPositionXYZ[3]; + double m_textColorRGB[3]; + double textSize; + + double m_lifeTime; + int m_itemUniqueId; +}; + + + class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface { CommonGraphicsApp* m_app; b3CriticalSection* m_cs; - + + public: GUIHelperInterface* m_childGuiHelper; + int m_uidGenerator; const unsigned char* m_texels; int m_textureWidth; int m_textureHeight; @@ -305,6 +335,7 @@ public: MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) :m_app(app) ,m_cs(0), + m_uidGenerator(0), m_texels(0), m_textureId(-1) { @@ -548,6 +579,96 @@ public: virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) { } + + + + + btAlignedObjectArray m_userDebugText; + + UserDebugText m_tmpText; + + virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) + { + + m_tmpText.m_itemUniqueId = m_uidGenerator++; + m_tmpText.m_lifeTime = lifeTime; + m_tmpText.textSize = size; + int len = strlen(txt); + strcpy(m_tmpText.m_text,txt); + m_tmpText.m_textPositionXYZ[0] = positionXYZ[0]; + m_tmpText.m_textPositionXYZ[1] = positionXYZ[1]; + m_tmpText.m_textPositionXYZ[2] = positionXYZ[2]; + m_tmpText.m_textColorRGB[0] = textColorRGB[0]; + m_tmpText.m_textColorRGB[1] = textColorRGB[1]; + m_tmpText.m_textColorRGB[2] = textColorRGB[2]; + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddText); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + + return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; + } + + btAlignedObjectArray m_userDebugLines; + UserDebugDrawLine m_tmpLine; + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) + { + m_tmpLine.m_lifeTime = lifeTime; + m_tmpLine.m_lineWidth = lineWidth; + m_tmpLine.m_itemUniqueId = m_uidGenerator++; + m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0]; + m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1]; + m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2]; + + m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0]; + m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1]; + m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2]; + + m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; + m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; + m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddLine); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; + } + + int m_removeDebugItemUid; + + virtual void removeUserDebugItem( int debugItemUniqueId) + { + m_removeDebugItemUid = debugItemUniqueId; + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugRemoveItem); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + + } + virtual void removeAllUserDebugItems( ) + { + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems); + m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } + + } + }; @@ -935,10 +1056,68 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->getCriticalSection()->unlock(); break; } + + case eGUIUserDebugAddText: + { + m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIUserDebugAddLine: + { + m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIUserDebugRemoveItem: + { + for (int i=0;im_userDebugLines.size();i++) + { + if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid) + { + m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1); + m_multiThreadedHelper->m_userDebugLines.pop_back(); + break; + } + } + + + for (int i=0;im_userDebugText.size();i++) + { + if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid) + { + m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1); + m_multiThreadedHelper->m_userDebugText.pop_back(); + break; + } + } + + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } + case eGUIUserDebugRemoveAllItems: + { + m_multiThreadedHelper->m_userDebugLines.clear(); + m_multiThreadedHelper->m_userDebugText.clear(); + m_multiThreadedHelper->m_uidGenerator = 0; + m_multiThreadedHelper->getCriticalSection()->lock(); + m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); + m_multiThreadedHelper->getCriticalSection()->unlock(); + break; + } case eGUIHelperIdle: + { + break; + } default: { - + btAssert(0); } } @@ -993,15 +1172,38 @@ void PhysicsServerExample::renderScene() //add array of lines //draw all user- 'text3d' messages - if (m_guiHelper) + if (m_multiThreadedHelper) { - btVector3 from(0, 0, 0); - btVector3 toX(1, 1, 1); - btVector3 color(0, 1, 0); - double width = 2; - m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, width); + + for (int i=0;im_userDebugLines.size();i++) + { + btVector3 from; + from.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]); + btVector3 toX; + toX.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]); + + btVector3 color; + color.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]); - m_guiHelper->getAppInterface()->drawText3D("hi", 1, 1, 1, 1); + + m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth); + } + + for (int i=0;im_userDebugText.size();i++) + { + m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text, + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2], + m_multiThreadedHelper->m_userDebugText[i].textSize); + + } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 82925c8f8..69b7916a1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -500,6 +500,40 @@ struct CreateJointArgs int m_jointType; }; + + +enum EnumUserDebugDrawFlags +{ + USER_DEBUG_HAS_LINE=1, + USER_DEBUG_HAS_TEXT=2, + USER_DEBUG_REMOVE_ONE_ITEM=4, + USER_DEBUG_REMOVE_ALL=8 +}; + +struct UserDebugDrawArgs +{ + double m_debugLineFromXYZ[3]; + double m_debugLineToXYZ[3]; + double m_debugLineColorRGB[3]; + double m_lineWidth; + + double m_lifeTime; + int m_removeItemUniqueId; + + char m_text[MAX_FILENAME_LENGTH]; + double m_textPositionXYZ[3]; + double m_textColorRGB[3]; + double m_textSize; +}; + + + +struct UserDebugDrawResultArgs +{ + int m_debugItemUniqueId; +}; + + struct SharedMemoryCommand { int m_type; @@ -536,6 +570,7 @@ struct SharedMemoryCommand struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments; struct LoadTextureArgs m_loadTextureArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; + struct UserDebugDrawArgs m_userDebugDrawArgs; struct LoadBunnyArgs m_loadBunnyArguments; }; }; @@ -584,6 +619,7 @@ struct SharedMemoryStatus struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs; struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; struct SendVisualShapeDataArgs m_sendVisualShapeArgs; + struct UserDebugDrawResultArgs m_userDebugDrawArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 47c7e7280..1c2e5835a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -40,6 +40,8 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_VISUAL_SHAPE_INFO, CMD_UPDATE_VISUAL_SHAPE, CMD_LOAD_TEXTURE, + CMD_USER_DEBUG_DRAW, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -100,6 +102,8 @@ enum EnumSharedMemoryServerStatus CMD_VISUAL_SHAPE_UPDATE_FAILED, CMD_LOAD_TEXTURE_COMPLETED, CMD_LOAD_TEXTURE_FAILED, + CMD_USER_DEBUG_DRAW_COMPLETED, + CMD_USER_DEBUG_DRAW_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -239,6 +243,7 @@ struct b3VisualShapeData char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN]; double m_localInertiaFrame[7];//pos[3], orn[4] //todo: add more data if necessary (material color etc, although material can be in asset file .obj file) + double m_rgbaColor[4]; }; struct b3VisualShapeInformation diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 9bf123413..643c7c1c0 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -527,7 +527,11 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1]; visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2]; visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3]; - + visualShape.m_rgbaColor[0] = rgbaColor[0]; + visualShape.m_rgbaColor[1] = rgbaColor[1]; + visualShape.m_rgbaColor[2] = rgbaColor[2]; + visualShape.m_rgbaColor[3] = rgbaColor[3]; + convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape); m_data->m_visualShapes.push_back(visualShape); @@ -872,10 +876,14 @@ int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int return m_data->m_textures.size()-1; } -void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) +int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) { int width,height,n; unsigned char* image=0; image = stbi_load(filename, &width, &height, &n, 3); - registerTexture(image, width, height); + if (image && (width>=0) && (height>=0)) + { + return registerTexture(image, width, height); + } + return -1; } \ No newline at end of file diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index afd0a2ae6..48c5c1079 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -38,7 +38,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void render(); void render(const float viewMat[16], const float projMat[16]); - void loadTextureFile(const char* filename); + int loadTextureFile(const char* filename); int registerTexture(unsigned char* texels, int width, int height); void activateShapeTexture(int shapeUniqueId, int textureUniqueId); void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); diff --git a/examples/SharedMemory/udp/main.cpp b/examples/SharedMemory/udp/main.cpp index 39ae4ea79..bc8c6c57d 100644 --- a/examples/SharedMemory/udp/main.cpp +++ b/examples/SharedMemory/udp/main.cpp @@ -94,7 +94,8 @@ int main(int argc, char *argv[]) switch (event.type) { - case ENET_EVENT_TYPE_CONNECT: + case ENET_EVENT_TYPE_CONNECT: + { printf("A new client connected from %x:%u.\n", event.peer->address.host, event.peer->address.port); @@ -103,8 +104,9 @@ int main(int argc, char *argv[]) event.peer->data = (void*)"Client information"; break; - + } case ENET_EVENT_TYPE_RECEIVE: + { if (gVerboseNetworkMessagesServer) { printf("A packet of length %u containing '%s' was " @@ -171,8 +173,9 @@ int main(int argc, char *argv[]) //enet_host_broadcast(server, 0, event.packet); break; - + } case ENET_EVENT_TYPE_DISCONNECT: + { printf("%s disconnected.\n", event.peer->data); /* Reset the peer's client information. */ @@ -180,6 +183,11 @@ int main(int argc, char *argv[]) event.peer->data = NULL; break; + } + default: + { + + } } } else if (serviceResult > 0) diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp index 7a046f9c4..49b59e444 100644 --- a/examples/SoftDemo/SoftDemo.cpp +++ b/examples/SoftDemo/SoftDemo.cpp @@ -2131,12 +2131,12 @@ void SoftDemo::initPhysics() for (int j=0;j MAX_SDF_BODIES) { + PyErr_SetString(SpamError, "loadBullet exceeds body capacity"); + return NULL; + } + + pylist = PyTuple_New(numBodies); + + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { + for (i = 0; i < numBodies; i++) { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; + + } static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args) @@ -366,7 +383,7 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { return v; } -#define MAX_SDF_BODIES 512 + static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { const char* sdfFileName = ""; @@ -1376,12 +1393,14 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { // // // Args: // vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { int i, len; PyObject* seq; + if (objVec==NULL) + return 0; - seq = PySequence_Fast(objMat, "expected a sequence"); - len = PySequence_Size(objMat); + seq = PySequence_Fast(objVec, "expected a sequence"); + len = PySequence_Size(objVec); if (len == 3) { for (i = 0; i < len; i++) { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); @@ -1397,7 +1416,9 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) { static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { int i, len; PyObject* seq; - + if (obVec==NULL) + return 0; + seq = PySequence_Fast(obVec, "expected a sequence"); len = PySequence_Size(obVec); if (len == 3) { @@ -1415,7 +1436,9 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { int i, len; PyObject* seq; - + if (obVec==NULL) + return 0; + seq = PySequence_Fast(obVec, "expected a sequence"); len = PySequence_Size(obVec); if (len == 4) { @@ -1429,6 +1452,196 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { return 0; } + + +static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) +{ + int size = PySequence_Size(args); + + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; + + PyObject* pyResultList = 0; + char* text; + double posXYZ[3]; + double colorRGB[3]={1,1,1}; + + + PyObject* textPositionObj=0; + PyObject* textColorRGBObj=0; + double textSize = 1.f; + double lifeTime = 0.f; + + static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime)) + { + return NULL; + } + + res = pybullet_internalSetVectord(textPositionObj,posXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + return NULL; + } + + if (textColorRGBObj) + { + res = pybullet_internalSetVectord(textColorRGBObj,colorRGB); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineTo[3]"); + return NULL; + } + } + + + commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + PyErr_SetString(SpamError, "Error in addUserDebugText."); + return NULL; +} + + +static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds) +{ + int size = PySequence_Size(args); + + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; + + PyObject* pyResultList = 0; + + double fromXYZ[3]; + double toXYZ[3]; + double colorRGB[3]={1,1,1}; + + PyObject* lineFromObj=0; + PyObject* lineToObj=0; + PyObject* lineColorRGBObj=0; + double lineWidth = 1.f; + double lifeTime = 0.f; + + static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime)) + { + return NULL; + } + + res = pybullet_internalSetVectord(lineFromObj,fromXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + return NULL; + } + + res = pybullet_internalSetVectord(lineToObj,toXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineTo[3]"); + return NULL; + } + if (lineColorRGBObj) + { + res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB); + } + + commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + PyErr_SetString(SpamError, "Error in addUserDebugLine."); + return NULL; +} + + + + +static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int itemUniqueId; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) { + PyErr_SetString(SpamError, "Error parsing user debug item unique id"); + return NULL; + } + + commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + Py_INCREF(Py_None); + return Py_None; +} + +static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitUserDebugDrawRemoveAll(sm); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + + Py_INCREF(Py_None); + return Py_None; +} + + + static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) { int size = PySequence_Size(args); @@ -1509,7 +1722,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) PyTuple_SetItem(visualShapeObList, 6, vec); } - + visualShapeInfo.m_visualShapeData[0].m_rgbaColor[0]; PyTuple_SetItem(pyResultList, i, visualShapeObList); } @@ -2687,6 +2900,30 @@ static PyMethodDef SpamMethods[] = { "axis-aligned bounding box volume (AABB)." "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]." }, + + + { "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS, + "Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. " + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." + }, + + + { "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS, + "Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds " + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." + }, + + { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, + "remove a user debug draw item, giving its unique id" + }, + + + { "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS, + "remove all user debug draw items" + }, + + + {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, "Return the visual shape information for one object." },