diff --git a/build_cmake_pybullet_double.sh b/build_cmake_pybullet_double.sh index a811d82ea..8817311f0 100755 --- a/build_cmake_pybullet_double.sh +++ b/build_cmake_pybullet_double.sh @@ -12,4 +12,16 @@ cd pybullet if [ -e pybullet.dylib ]; then ln -f -s pybullet.dylib pybullet.so fi +if [ -e pybullet_envs ]; then + rm pybullet_envs +fi +if [ -e pybullet_data ]; then + rm pybullet_data +fi +if [ -e pybullet_utils ]; then + rm pybullet_utils +fi +ln -s ../../../examples/pybullet/gym/pybullet_envs . +ln -s ../../../examples/pybullet/gym/pybullet_data . +ln -s ../../../examples/pybullet/gym/pybullet_utils . echo "Completed build of Bullet." diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index e686bfda6..024fc1ceb 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -217,6 +217,11 @@ void ExampleBrowserThreadFunc(void* userPtr, void* lsMemory) ExampleBrowserArgs* args = (ExampleBrowserArgs*)userPtr; //int workLeft = true; b3CommandLineArgs args2(args->m_argc, args->m_argv); + int minUpdateMs = 4000; + if (args2.GetCmdLineArgument("minGraphicsUpdateTimeMs", minUpdateMs)) + { + gMinUpdateTimeMicroSecs = minUpdateMs; + } b3Clock clock; ExampleEntriesPhysicsServer examples; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 094683da0..a0c3c4fd4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1604,11 +1604,30 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3Physics command->m_createMultiBodyArgs.m_bodyName[0] = 0; command->m_createMultiBodyArgs.m_baseLinkIndex = -1; command->m_createMultiBodyArgs.m_numLinks = 0; + command->m_createMultiBodyArgs.m_numBatchObjects = 0; return (b3SharedMemoryCommandHandle)command; } return 0; } +//batch creation is an performance feature to create a large number of multi bodies in one command +B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + if (cl && command->m_type == CMD_CREATE_MULTI_BODY) + { + command->m_createMultiBodyArgs.m_numBatchObjects = numBatchObjects; + cl->uploadBulletFileToSharedMemory((const char*)batchPositions, sizeof(double) * 3 * numBatchObjects); + } + return 0; +} + B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, const double basePosition[3], const double baseOrientation[4], const double baseInertialFramePosition[3], const double baseInertialFrameOrientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2c695e0e9..01529410b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -511,6 +511,9 @@ extern "C" int linkJointType, const double linkJointAxis[/*3*/]); + //batch creation is an performance feature to create a large number of multi bodies in one command + B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects); + //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 140fe772d..30bca5b4c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2621,7 +2621,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() #endif //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it - m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768); + m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024); m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); @@ -4401,7 +4401,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str btVector3 v2(vertexUpload[i2*3+0], vertexUpload[i2*3+1], vertexUpload[i2*3+2]); - meshInterface->addTriangle(v0, v1, v2); + meshInterface->addTriangle(v0*meshScale, v1*meshScale, v2*meshScale); } } @@ -4434,7 +4434,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str btVector3 pt(vertexUpload[v*3+0], vertexUpload[v*3+1], vertexUpload[v*3+2]); - convexHull->addPoint(pt, false); + convexHull->addPoint(pt*meshScale, false); } convexHull->recalcLocalAabb(); @@ -7058,6 +7058,36 @@ bool PhysicsServerCommandProcessor::processLoadSDFCommand(const struct SharedMem bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { + if (clientCmd.m_createMultiBodyArgs.m_numBatchObjects > 0) + { + //batch of objects, to speed up creation time + bool result = false; + SharedMemoryCommand clientCmd2 = clientCmd; + int baseLinkIndex = clientCmd.m_createMultiBodyArgs.m_baseLinkIndex; + double* basePositionAndOrientations = (double*)bufferServerToClient; + for (int i = 0; i < clientCmd2.m_createMultiBodyArgs.m_numBatchObjects; i++) + { + clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 0] = basePositionAndOrientations[0 + i * 3]; + clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 1] = basePositionAndOrientations[1 + i * 3]; + clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 2] = basePositionAndOrientations[2 + i * 3]; + if (i == (clientCmd2.m_createMultiBodyArgs.m_numBatchObjects - 1)) + { + result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + } + else + { + result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, 0, 0); + } + } + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + return result; + } + return processCreateMultiBodyCommandSingle(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); +} + +bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + BT_PROFILE("processCreateMultiBodyCommand2"); bool hasStatus = true; serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; @@ -7082,10 +7112,16 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S - bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + + bool ok = 0; + { + BT_PROFILE("processImportedObjects"); + ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + } if (ok) { + BT_PROFILE("post process"); int bodyUniqueId = -1; if (m_data->m_sdfRecentLoadedBodies.size() == 1) @@ -7095,15 +7131,23 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S m_data->m_sdfRecentLoadedBodies.clear(); if (bodyUniqueId >= 0) { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; + if (bufferSizeInBytes>0 && serverStatusOut.m_numDataStreamBytes==0) + { + { + BT_PROFILE("autogenerateGraphicsObjects"); + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + } + + BT_PROFILE("createBodyInfoStream"); + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; - - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + } } } @@ -8209,6 +8253,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st } } + if (clientCmd.m_updateFlags & SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = clientCmd.m_physSimParamArgs.m_minimumSolverIslandSize; + } + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) { m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; @@ -10977,6 +11026,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } + case CMD_CREATE_MULTI_BODY: { hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 2e08cb713..237309b77 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -41,6 +41,8 @@ protected: bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0853eeb7d..1c2b9e00b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -980,14 +980,8 @@ struct b3CreateMultiBodyArgs int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; double m_linkJointAxis[3 * MAX_CREATE_MULTI_BODY_LINKS]; int m_flags; -#if 0 - std::string m_name; - std::string m_sourceFile; - btTransform m_rootTransformInWorld; - btHashMap m_materials; - btHashMap m_links; - btHashMap m_joints; -#endif + int m_numBatchObjects; + }; struct b3CreateMultiBodyResultArgs diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index f04d43f18..284430847 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -2184,6 +2184,21 @@ int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorC command = b3CreateMultiBodyCommandInit(sm); + if (args.m_useMaximalCoordinates) + { + b3CreateMultiBodyUseMaximalCoordinates(command); + } + if (args.m_batchPositions.size()) + { + btAlignedObjectArray positionArray; + for (int i = 0; i < args.m_batchPositions.size(); i++) + { + positionArray.push_back(args.m_batchPositions[i][0]); + positionArray.push_back(args.m_batchPositions[i][1]); + positionArray.push_back(args.m_batchPositions[i][2]); + } + b3CreateMultiBodySetBatchPositions(sm, command, &positionArray[0], args.m_batchPositions.size()); + } baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex, doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 7e9c56c14..cca09d974 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -418,7 +418,7 @@ struct b3RobotSimulatorCreateMultiBodyArgs int *m_linkParentIndices; int *m_linkJointTypes; btVector3 *m_linkJointAxes; - + btAlignedObjectArray m_batchPositions; int m_useMaximalCoordinates; b3RobotSimulatorCreateMultiBodyArgs() diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 4e7581ee2..1b791ec7e 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -13,8 +13,24 @@ IF(BUILD_PYBULLET_NUMPY) ) ENDIF() +ADD_DEFINITIONS(-DSTATIC_LINK_SPD_PLUGIN) + SET(pybullet_SRCS pybullet.c + ../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h + ../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h + ../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h + ../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h + ../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h + ../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h + ../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp + ../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h ../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h diff --git a/examples/pybullet/examples/createMultiBodyBatch.py b/examples/pybullet/examples/createMultiBodyBatch.py new file mode 100644 index 000000000..3e8d33cab --- /dev/null +++ b/examples/pybullet/examples/createMultiBodyBatch.py @@ -0,0 +1,150 @@ +import pybullet as p +import time +import math + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") + + +p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) +p.setTimeStep(1./120.) +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") +#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) +p.loadURDF("plane100.urdf", useMaximalCoordinates=True) +#disable rendering during creation. +p.setPhysicsEngineParameter(contactBreakingThreshold=0.04) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) +#disable tinyrenderer, software (CPU) renderer, we don't use it here +p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) + +shift = [0,-0.02,0] +meshScale=[0.1,0.1,0.1] + +vertices=[ +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[1.000000,1.000000,1.000000], +[-1.000000,1.000000,1.000000], +[-1.000000,-1.000000,-1.000000], +[1.000000,-1.000000,-1.000000], +[1.000000,1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,-1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,1.000000,1.000000], +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,-1.000000], +[1.000000,1.000000,-1.000000], +[1.000000,1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[-1.000000,-1.000000,-1.000000], +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[1.000000,-1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,1.000000,1.000000], +[1.000000,1.000000,1.000000], +[1.000000,1.000000,-1.000000] +] + +normals=[ +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000] +] + +uvs=[ +[0.750000,0.250000], +[1.000000,0.250000], +[1.000000,0.000000], +[0.750000,0.000000], +[0.500000,0.250000], +[0.250000,0.250000], +[0.250000,0.000000], +[0.500000,0.000000], +[0.500000,0.000000], +[0.750000,0.000000], +[0.750000,0.250000], +[0.500000,0.250000], +[0.250000,0.500000], +[0.250000,0.250000], +[0.000000,0.250000], +[0.000000,0.500000], +[0.250000,0.500000], +[0.250000,0.250000], +[0.500000,0.250000], +[0.500000,0.500000], +[0.000000,0.000000], +[0.000000,0.250000], +[0.250000,0.250000], +[0.250000,0.000000] +] +indices=[ 0, 1, 2, 0, 2, 3, #//ground face + 6, 5, 4, 7, 6, 4, #//top face + 10, 9, 8, 11, 10, 8, + 12, 13, 14, 12, 14, 15, + 18, 17, 16, 19, 18, 16, + 20, 21, 22, 20, 22, 23] + +#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) +#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) +visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) +#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) + +#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") +collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) + +texUid = p.loadTexture("tex256.png") + + +batchPositions=[] + + + +for x in range (33): + for y in range (34): + for z in range (4): + batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5]) + +bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True) +p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid) + +#p.syncBodyInfo() +#print("numBodies=",p.getNumBodies()) +p.stopStateLogging(logId) +p.setGravity(0,0,-10) + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) + +colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] +currentColor = 0 + + +while (1): + p.stepSimulation() + #time.sleep(1./120.) + #p.getCameraImage(320,200) + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/DeepMimic_Optimizer.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/DeepMimic_Optimizer.py index ff4eaa79f..9aaeb2b90 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/DeepMimic_Optimizer.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/DeepMimic_Optimizer.py @@ -11,7 +11,7 @@ print("parentdir=",parentdir) from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv from pybullet_envs.deep_mimic.learning.rl_world import RLWorld from pybullet_utils.logger import Logger -from testrl import update_world, update_timestep, build_world +from pybullet_envs.deep_mimic.testrl import update_world, update_timestep, build_world import pybullet_utils.mpi_util as MPIUtil args = [] diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/__init__.py @@ -0,0 +1 @@ + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/env.py index 4c8973674..c8c17df5d 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/env.py @@ -1,4 +1,10 @@ -from abc import ABC, abstractmethod +from abc import abstractmethod +import sys, abc +if sys.version_info >= (3, 4): + ABC = abc.ABC +else: + ABC = abc.ABCMeta('ABC', (), {}) + import numpy as np from enum import Enum @@ -10,4 +16,4 @@ class Env(ABC): def __init__(self, args, enable_draw): self.enable_draw = enable_draw - return \ No newline at end of file + return diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py index 3159fcb29..35f618cc8 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py @@ -27,6 +27,8 @@ class PyBulletDeepMimicEnv(Env): if not self._isInitialized: if self.enable_draw: self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI) + #disable 'GUI' since it slows down a lot on Mac OSX and some other platforms + self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_GUI,0) else: self._pybullet_client = bullet_client.BulletClient() diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/__init__.py @@ -0,0 +1 @@ + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/agent_builder.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/agent_builder.py index ca54f46d5..e2047da98 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/agent_builder.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/agent_builder.py @@ -1,6 +1,6 @@ import json import numpy as np -from learning.ppo_agent import PPOAgent +from pybullet_envs.deep_mimic.learning.ppo_agent import PPOAgent import pybullet_data AGENT_TYPE_KEY = "AgentType" diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/fc_2layers_1024units.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/fc_2layers_1024units.py index 20f5c0ffa..b4d466124 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/fc_2layers_1024units.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/fc_2layers_1024units.py @@ -1,5 +1,5 @@ import tensorflow as tf -import learning.tf_util as TFUtil +import pybullet_envs.deep_mimic.learning.tf_util as TFUtil NAME = "fc_2layers_1024units" diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/net_builder.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/net_builder.py index 1b5983a1b..32328493b 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/net_builder.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/net_builder.py @@ -1,4 +1,4 @@ -import learning.nets.fc_2layers_1024units as fc_2layers_1024units +import pybullet_envs.deep_mimic.learning.nets.fc_2layers_1024units as fc_2layers_1024units def build_net(net_name, input_tfs, reuse=False): net = None diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/pg_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/pg_agent.py index 1f82b8553..75d1bae9a 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/pg_agent.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/pg_agent.py @@ -2,12 +2,12 @@ import numpy as np import tensorflow as tf import copy -from learning.tf_agent import TFAgent -from learning.solvers.mpi_solver import MPISolver -import learning.tf_util as TFUtil -import learning.nets.net_builder as NetBuilder -from learning.tf_normalizer import TFNormalizer -import learning.rl_util as RLUtil +from pybullet_envs.deep_mimic.learning.tf_agent import TFAgent +from pybullet_envs.deep_mimic.learning.solvers.mpi_solver import MPISolver +import pybullet_envs.deep_mimic.learning.tf_util as TFUtil +import pybullet_envs.deep_mimic.learning.nets.net_builder as NetBuilder +from pybullet_envs.deep_mimic.learning.tf_normalizer import TFNormalizer +import pybullet_envs.deep_mimic.learning.rl_util as RLUtil from pybullet_utils.logger import Logger import pybullet_utils.mpi_util as MPIUtil import pybullet_utils.math_util as MathUtil @@ -350,4 +350,4 @@ class PGAgent(TFAgent): def _build_replay_buffer(self, buffer_size): super()._build_replay_buffer(buffer_size) self.replay_buffer.add_filter_key(self.EXP_ACTION_FLAG) - return \ No newline at end of file + return diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py index ece705302..ff66ba004 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py @@ -2,10 +2,10 @@ import numpy as np import copy as copy import tensorflow as tf -from learning.pg_agent import PGAgent -from learning.solvers.mpi_solver import MPISolver -import learning.tf_util as TFUtil -import learning.rl_util as RLUtil +from pybullet_envs.deep_mimic.learning.pg_agent import PGAgent +from pybullet_envs.deep_mimic.learning.solvers.mpi_solver import MPISolver +import pybullet_envs.deep_mimic.learning.tf_util as TFUtil +import pybullet_envs.deep_mimic.learning.rl_util as RLUtil from pybullet_utils.logger import Logger import pybullet_utils.mpi_util as MPIUtil import pybullet_utils.math_util as MathUtil @@ -365,4 +365,4 @@ class PPOAgent(PGAgent): self._actor_stepsize_ph: stepsize, } self.sess.run(self._actor_stepsize_update_op, feed) - return \ No newline at end of file + return diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py index 3e66eb270..ce6b2a9f4 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py @@ -2,14 +2,20 @@ import numpy as np import copy import os import time +import sys +from abc import abstractmethod +import abc +if sys.version_info >= (3, 4): + ABC = abc.ABC +else: + ABC = abc.ABCMeta('ABC', (), {}) -from abc import ABC, abstractmethod from enum import Enum -from learning.path import * -from learning.exp_params import ExpParams -from learning.normalizer import Normalizer -from learning.replay_buffer import ReplayBuffer +from pybullet_envs.deep_mimic.learning.path import * +from pybullet_envs.deep_mimic.learning.exp_params import ExpParams +from pybullet_envs.deep_mimic.learning.normalizer import Normalizer +from pybullet_envs.deep_mimic.learning.replay_buffer import ReplayBuffer from pybullet_utils.logger import Logger import pybullet_utils.mpi_util as MPIUtil import pybullet_utils.math_util as MathUtil @@ -161,7 +167,7 @@ class RLAgent(ABC): return self._enable_training def set_enable_training(self, enable): - print("set_enable_training!=", enable) + print("set_enable_training=", enable) self._enable_training = enable if (self._enable_training): self.reset() @@ -593,4 +599,4 @@ class RLAgent(ABC): self.logger.log_tabular("Exp_Rate", self.exp_params_curr.rate) self.logger.log_tabular("Exp_Noise", self.exp_params_curr.noise) self.logger.log_tabular("Exp_Temp", self.exp_params_curr.temp) - return \ No newline at end of file + return diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_world.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_world.py index 3fd7bee76..a1761bc30 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_world.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_world.py @@ -1,7 +1,7 @@ import numpy as np -import learning.agent_builder as AgentBuilder -import learning.tf_util as TFUtil -from learning.rl_agent import RLAgent +import pybullet_envs.deep_mimic.learning.agent_builder as AgentBuilder +import pybullet_envs.deep_mimic.learning.tf_util as TFUtil +from pybullet_envs.deep_mimic.learning.rl_agent import RLAgent from pybullet_utils.logger import Logger import pybullet_data diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/__init__.py @@ -0,0 +1 @@ + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/mpi_solver.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/mpi_solver.py index 46c0963fa..f2d18051c 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/mpi_solver.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/mpi_solver.py @@ -1,12 +1,12 @@ from mpi4py import MPI import tensorflow as tf import numpy as np -import learning.tf_util as TFUtil +import pybullet_envs.deep_mimic.learning.tf_util as TFUtil import pybullet_utils.math_util as MathUtil import pybullet_utils.mpi_util as MPIUtil from pybullet_utils.logger import Logger -from learning.solvers.solver import Solver +from pybullet_envs.deep_mimic.learning.solvers.solver import Solver class MPISolver(Solver): CHECK_SYNC_ITERS = 1000 diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/solver.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/solver.py index 8df65c029..cd2765272 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/solver.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/solvers/solver.py @@ -1,4 +1,9 @@ -from abc import ABC, abstractmethod +from abc import abstractmethod +import sys, abc +if sys.version_info >= (3, 4): + ABC = abc.ABC +else: + ABC = abc.ABCMeta('ABC', (), {}) class Solver(ABC): def __init__(self, vars): @@ -7,4 +12,4 @@ class Solver(ABC): @abstractmethod def update(self, grads): - pass \ No newline at end of file + pass diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_agent.py index 51dc5e88a..95d707140 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_agent.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_agent.py @@ -2,9 +2,9 @@ import numpy as np import tensorflow as tf from abc import abstractmethod -from learning.rl_agent import RLAgent +from pybullet_envs.deep_mimic.learning.rl_agent import RLAgent from pybullet_utils.logger import Logger -from learning.tf_normalizer import TFNormalizer +from pybullet_envs.deep_mimic.learning.tf_normalizer import TFNormalizer class TFAgent(RLAgent): RESOURCE_SCOPE = 'resource' diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_normalizer.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_normalizer.py index ecbddc95b..82f5745f0 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_normalizer.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/tf_normalizer.py @@ -1,7 +1,7 @@ import numpy as np import copy import tensorflow as tf -from learning.normalizer import Normalizer +from pybullet_envs.deep_mimic.learning.normalizer import Normalizer class TFNormalizer(Normalizer): diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py index 31b976563..39c015f6b 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py @@ -6,7 +6,7 @@ os.sys.path.insert(0,parentdir) print("parentdir=",parentdir) import json from pybullet_envs.deep_mimic.learning.rl_world import RLWorld -from learning.ppo_agent import PPOAgent +from pybullet_envs.deep_mimic.learning.ppo_agent import PPOAgent import pybullet_data from pybullet_utils.arg_parser import ArgParser diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 06561ab1f..f3e01e5dc 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1501,9 +1501,32 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int minimumSolverIslandSize = -1; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching", "restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", + "numSolverIterations", + "useSplitImpulse", + "splitImpulsePenetrationThreshold", + "numSubSteps", + "collisionFilterMode", + "contactBreakingThreshold", + "maxNumCmdPer1ms", + "enableFileCaching", + "restitutionVelocityThreshold", + "erp", + "contactERP", + "frictionERP", + "enableConeFriction", + "deterministicOverlappingPairs", + "allowedCcdPenetration", + "jointFeedbackMode", + "solverResidualThreshold", + "contactSlop", + "enableSAT", + "constraintSolverType", + "globalCFM", + "minimumSolverIslandSize", + "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId)) { return NULL; @@ -7656,18 +7679,19 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* linkJointAxisObj = 0; PyObject* linkInertialFramePositionObj = 0; PyObject* linkInertialFrameOrientationObj = 0; + PyObject* objBatchPositions = 0; static char* kwlist[] = { "baseMass", "baseCollisionShapeIndex", "baseVisualShapeIndex", "basePosition", "baseOrientation", "baseInertialFramePosition", "baseInertialFrameOrientation", "linkMasses", "linkCollisionShapeIndices", "linkVisualShapeIndices", "linkPositions", "linkOrientations", "linkInertialFramePositions", "linkInertialFrameOrientations", "linkParentIndices", - "linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "physicsClientId", NULL}; + "linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "batchPositions", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiii", kwlist, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiiOi", kwlist, &baseMass, &baseCollisionShapeIndex, &baseVisualShapeIndex, &basePosObj, &baseOrnObj, &baseInertialFramePositionObj, &baseInertialFrameOrientationObj, &linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, &linkInertialFramePositionObj, &linkInertialFrameOrientationObj, &linkParentIndicesObj, - &linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &physicsClientId)) + &linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &objBatchPositions, &physicsClientId)) { return NULL; } @@ -7690,6 +7714,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int numLinkJoinAxis = linkJointAxisObj ? PySequence_Size(linkJointAxisObj) : 0; int numLinkInertialFramePositions = linkInertialFramePositionObj ? PySequence_Size(linkInertialFramePositionObj) : 0; int numLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Size(linkInertialFrameOrientationObj) : 0; + int numBatchPositions = objBatchPositions ? PySequence_Size(objBatchPositions) : 0; PyObject* seqLinkMasses = linkMassesObj ? PySequence_Fast(linkMassesObj, "expected a sequence") : 0; PyObject* seqLinkCollisionShapes = linkCollisionShapeIndicesObj ? PySequence_Fast(linkCollisionShapeIndicesObj, "expected a sequence") : 0; @@ -7702,6 +7727,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* seqLinkInertialFramePositions = linkInertialFramePositionObj ? PySequence_Fast(linkInertialFramePositionObj, "expected a sequence") : 0; PyObject* seqLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Fast(linkInertialFrameOrientationObj, "expected a sequence") : 0; + PyObject* seqBatchPositions = objBatchPositions ? PySequence_Fast(objBatchPositions, "expected a sequence") : 0; + if ((numLinkMasses == numLinkCollisionShapes) && (numLinkMasses == numLinkVisualShapes) && (numLinkMasses == numLinkPositions) && @@ -7728,6 +7755,18 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje baseIndex = b3CreateMultiBodyBase(commandHandle, baseMass, baseCollisionShapeIndex, baseVisualShapeIndex, basePosition, baseOrientation, baseInertialFramePosition, baseInertialFrameOrientation); + if (numBatchPositions > 0) + { + double* batchPositions = malloc(sizeof(double) * 3 * numBatchPositions); + for (i = 0; i < numBatchPositions; i++) + { + pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3*i]); + } + b3CreateMultiBodySetBatchPositions(sm, commandHandle, batchPositions, numBatchPositions); + free(batchPositions); + } + + for (i = 0; i < numLinkMasses; i++) { double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i); @@ -7742,7 +7781,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int linkJointType; pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions, i, linkInertialFramePosition); - pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj, i, linkInertialFrameOrientation); + pybullet_internalGetVector4FromSequence(seqLinkInertialFrameOrientations, i, linkInertialFrameOrientation); pybullet_internalGetVector3FromSequence(seqLinkPositions, i, linkPosition); pybullet_internalGetVector4FromSequence(seqLinkOrientations, i, linkOrientation); pybullet_internalGetVector3FromSequence(seqLinkJoinAxis, i, linkJointAxis); @@ -7782,6 +7821,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje Py_DECREF(seqLinkInertialFramePositions); if (seqLinkInertialFrameOrientations) Py_DECREF(seqLinkInertialFrameOrientations); + if (seqBatchPositions) + Py_DECREF(seqBatchPositions); if (useMaximalCoordinates > 0) { @@ -7822,7 +7863,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje Py_DECREF(seqLinkInertialFramePositions); if (seqLinkInertialFrameOrientations) Py_DECREF(seqLinkInertialFrameOrientations); - + if (seqBatchPositions) + Py_DECREF(seqBatchPositions); PyErr_SetString(SpamError, "All link arrays need to be same size."); return NULL; }