diff --git a/build_and_run_cmake_pybullet_double.sh b/build_and_run_cmake_pybullet_double.sh new file mode 100755 index 000000000..e05d3f06d --- /dev/null +++ b/build_and_run_cmake_pybullet_double.sh @@ -0,0 +1,6 @@ +#!/bin/sh +rm CMakeCache.txt +mkdir build_cmake +cd build_cmake +cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release .. +make -j12 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 813457225..583f38ba9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1252,7 +1252,41 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu } } +b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_LOAD_TEXTURE; + int len = strlen(filename); + if (lenm_loadTextureArguments.m_textureFileName,filename); + } else + { + command->m_loadTextureArguments.m_textureFileName[0] = 0; + } + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_UPDATE_VISUAL_SHAPE; + command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId; + command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex; + command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex; + command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2d2a4460c..830717467 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -99,6 +99,8 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA); void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); +b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index afb0e1fb8..b0fe2be97 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -997,14 +997,20 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { b3Warning("Cannot get visual shape information"); } - + if (statusType == CMD_VISUAL_SHAPE_UPDATE_FAILED) + { + b3Warning("Cannot update visual shape"); + } if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { b3VisualShapeInformation shapeInfo; b3GetVisualShapeInformation(m_physicsClientHandle, &shapeInfo); b3Printf("Num visual shapes: %d", shapeInfo.m_numVisualShapes); } - + if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) + { + b3Printf("Visual shape update completed."); + } if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { b3ContactInformation contactPointData; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 9dda6aca1..334806ff5 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -657,6 +657,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Visual Shape Info Request failed"); break; } + case CMD_VISUAL_SHAPE_UPDATE_COMPLETED: + { + break; + } + case CMD_VISUAL_SHAPE_UPDATE_FAILED: + { + b3Warning("Visual Shape Update failed"); + break; + } + case CMD_LOAD_TEXTURE_COMPLETED: + { + break; + } + case CMD_LOAD_TEXTURE_FAILED: + { + b3Warning("Load texture failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 613bc0234..e56ed672e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3001,6 +3001,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_UPDATE_VISUAL_SHAPE: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; + + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; + hasStatus = true; + + break; + } + case CMD_LOAD_TEXTURE: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + + m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); + + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + hasStatus = true; + + break; + } + default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5a1141821..50b099816 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -157,6 +157,19 @@ struct RequestVisualShapeDataArgs int m_startingVisualShapeIndex; }; +struct UpdateVisualShapeDataArgs +{ + int m_bodyUniqueId; + int m_jointIndex; + int m_shapeIndex; + int m_textureUniqueId; +}; + +struct LoadTextureArgs +{ + char m_textureFileName[MAX_FILENAME_LENGTH]; +}; + struct SendVisualShapeDataArgs { int m_bodyUniqueId; @@ -477,6 +490,8 @@ struct SharedMemoryCommand struct CreateJointArgs m_createJointArguments; struct RequestContactDataArgs m_requestContactPointArguments; struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; + struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments; + struct LoadTextureArgs m_loadTextureArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index c668e5fe8..b435a38a5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -35,6 +35,8 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_CONTACT_POINT_INFORMATION, CMD_SAVE_WORLD, CMD_REQUEST_VISUAL_SHAPE_INFO, + CMD_UPDATE_VISUAL_SHAPE, + CMD_LOAD_TEXTURE, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -81,6 +83,10 @@ enum EnumSharedMemoryServerStatus CMD_SAVE_WORLD_FAILED, CMD_VISUAL_SHAPE_INFO_COMPLETED, CMD_VISUAL_SHAPE_INFO_FAILED, + CMD_VISUAL_SHAPE_UPDATE_COMPLETED, + CMD_VISUAL_SHAPE_UPDATE_FAILED, + CMD_LOAD_TEXTURE_COMPLETED, + CMD_LOAD_TEXTURE_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 4ef4b0efc..9bf123413 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -33,6 +33,8 @@ subject to the following restrictions: #include #include "../Importers/ImportURDFDemo/UrdfParser.h" #include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData +#include "../TinyRenderer/model.h" +#include "../ThirdPartyLibs/stb_image/stb_image.h" enum MyFileType @@ -68,6 +70,7 @@ struct TinyRendererVisualShapeConverterInternalData int m_swWidth; int m_swHeight; TGAImage m_rgbColorBuffer; + b3AlignedObjectArray m_textures; b3AlignedObjectArray m_depthBuffer; b3AlignedObjectArray m_segmentationMaskBuffer; @@ -827,3 +830,52 @@ void TinyRendererVisualShapeConverter::resetAll() m_data->m_swRenderInstances.clear(); } +void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) +{ + btAssert(textureUniqueId < m_data->m_textures.size()); + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId); + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height); + } +} + +void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) +{ + int start = -1; + for (int i = 0; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex) + { + start = i; + break; + } + } + + if (start >= 0) + { + if (start + shapeIndex < m_data->m_visualShapes.size()) + { + activateShapeTexture(start + shapeIndex, textureUniqueId); + } + } +} + +int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) +{ + MyTexture2 texData; + texData.m_width = width; + texData.m_height = height; + texData.textureData = texels; + m_data->m_textures.push_back(texData); + return m_data->m_textures.size()-1; +} + +void 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); +} \ No newline at end of file diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index df490716b..afd0a2ae6 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -37,6 +37,11 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void render(); void render(const float viewMat[16], const float projMat[16]); + + void 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/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1af256b47..2cbd35530 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1358,8 +1358,82 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) return Py_None; } +static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + int objectUniqueId = -1; + int jointIndex = -1; + int shapeIndex = -1; + int textureUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (size == 4) + { + if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) { + PyErr_SetString(SpamError, "Error parsing object unique id, or joint index, or shape index, or texture unique id"); + return NULL; + } + + commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error resetting visual shape info"); + return NULL; + } + } + else + { + PyErr_SetString(SpamError, "setVisualShapeData requires 4 argument"); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} - +static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + const char* filename = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (size == 1) + { + if (!PyArg_ParseTuple(args, "s", &filename)) { + PyErr_SetString(SpamError, "Error parsing file name"); + return NULL; + } + + commandHandle = b3InitLoadTexture(sm, filename); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_LOAD_TEXTURE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error loading texture"); + return NULL; + } + } + else + { + PyErr_SetString(SpamError, "loadTexture requires 1 argument"); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { int size = PySequence_Size(args); @@ -2294,9 +2368,15 @@ static PyMethodDef SpamMethods[] = { "object-object collisions. Optional arguments one or two object unique " "ids, that need to be involved in the contact."}, - { "getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, + {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, "Return the visual shape information for one object." }, + {"resetVisualShapeData", pybullet_resetVisualShapeData, METH_VARARGS, + "Reset part of the visual shape information for one object." }, + + {"loadTexture", pybullet_loadTexture, METH_VARARGS, + "Load texture file." }, + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " "quaternion [x,y,z,w]"},