diff --git a/MANIFEST.in b/MANIFEST.in index 476648ddc..71611888c 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -8,5 +8,6 @@ recursive-include src *.hpp recursive-include examples/pybullet/gym *.* include examples/ThirdPartyLibs/enet/unix.c include examples/OpenGLWindow/*.* +recursive-include examples/SharedMemory/plugins *.* recursive-include examples/ThirdPartyLibs/glad *.* include examples/ThirdPartyLibs/enet/win32.c diff --git a/build3/premake4.lua b/build3/premake4.lua index fb94d3c5a..89936d8bf 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -399,8 +399,8 @@ end else xcodebuildsettings { - 'ARCHS = "$(ARCHS_STANDARD_32_BIT) $(ARCHS_STANDARD_64_BIT)"', - 'VALID_ARCHS = "x86_64 i386"', + 'ARCHS = "$(ARCHS_STANDARD_64_BIT)"', + 'VALID_ARCHS = "x86_64"', -- 'SDKROOT = "macosx10.9"', } end diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index c434113ca..02a469dc0 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -1,5 +1,3 @@ - - #include "fileIOPlugin.h" #include "../../SharedMemoryPublic.h" #include "../b3PluginContext.h" @@ -46,9 +44,14 @@ struct InMemoryFileIO : public CommonFileIOInterface { b3HashMap m_fileCache; InMemoryFileAccessor m_fileHandles[B3_MAX_FILEIO_INTERFACES]; + int m_numAllocs; + int m_numFrees; InMemoryFileIO() { + m_numAllocs=0; + m_numFrees=0; + for (int i=0;im_buffer); + m_numFrees++; delete (mem); + m_numFrees++; } } } @@ -80,6 +88,7 @@ struct InMemoryFileIO : public CommonFileIOInterface char* buffer = 0; if (len) { + m_numAllocs++; buffer = new char[len]; } return buffer; @@ -92,6 +101,7 @@ struct InMemoryFileIO : public CommonFileIOInterface virtual int registerFile(const char* fileName, char* buffer, int len) { + m_numAllocs++; InMemoryFile* f = new InMemoryFile(); f->m_buffer = buffer; f->m_fileSize = len; @@ -227,7 +237,6 @@ struct InMemoryFileIO : public CommonFileIOInterface } if (c && c!='\n') { - char a='\r'; if (c!=13) { destBuffer[numRead++]=c; @@ -347,8 +356,12 @@ struct WrapperFileIO : public CommonFileIOInterface if (slot>=0) { //first check the cache - int childHandle = m_cachedFiles.fileOpen(fileName, mode); - if (childHandle<0) + int cacheHandle = m_cachedFiles.fileOpen(fileName, mode); + if (cacheHandle>=0) + { + m_cachedFiles.fileClose(cacheHandle); + } + if (cacheHandle<0) { for (int i=0;i=0 && fileHandle=0 && fileHandle=0 && fileHandlem_text) + if (strlen(arguments->m_text)) { obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text)); } diff --git a/setup.py b/setup.py index 738c9339d..3fadd91b9 100644 --- a/setup.py +++ b/setup.py @@ -581,7 +581,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.3.0', + version='2.3.2', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 60ebfc9ee..a890da46f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1711,7 +1711,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, - btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_r1, btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m) const { @@ -1730,9 +1730,20 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link, v_ptr += num_links + 1; btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - scratch_r.resize(m_dofCount); - btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0; + //scratch_r.resize(m_dofCount); + //btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0; + scratch_r1.resize(m_dofCount+num_links); + btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0; + btScalar* links = num_links? &scratch_r1[m_dofCount] : 0; + int numLinksChildToRoot=0; + int l = link; + while (l != -1) + { + links[numLinksChildToRoot++]=l; + l = m_links[l].m_parent; + } + btMatrix3x3 *rot_from_world = &scratch_m[0]; const btVector3 p_minus_com_world = contact_point - m_basePos; @@ -1766,14 +1777,14 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link, // Qdot coefficients, if necessary. if (num_links > 0 && link > -1) { - // TODO: speed this up -- don't calculate for m_links we don't need. - // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, + // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, // which is resulting in repeated work being done...) // calculate required normals & positions in the local frames. - for (int i = 0; i < num_links; ++i) - { - // transform to local frame + for (int a = 0; a < numLinksChildToRoot; a++) + { + int i = links[numLinksChildToRoot-1-a]; + // transform to local frame const int parent = m_links[i].m_parent; const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];