diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ad1aa1aa..e4cdcc313 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG") #MESSAGE("CMAKE_CXX_FLAGS_DEBUG="+${CMAKE_CXX_FLAGS_DEBUG}) OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF) +SET(CLAMP_VELOCITIES "0" CACHE STRING "Clamp rigid bodies' velocity to this value, if larger than zero. Useful to prevent floating point errors or in general runaway velocities in complex scenarios") OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON) OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF) OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON) @@ -211,6 +212,10 @@ IF (INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) ADD_DEFINITIONS( -DBT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) ENDIF (INTERNAL_UPDATE_SERIALIZATION_STRUCTURES) +IF (CLAMP_VELOCITIES) +ADD_DEFINITIONS( -DBT_CLAMP_VELOCITY_TO=${CLAMP_VELOCITIES}) +ENDIF (CLAMP_VELOCITIES) + IF (USE_DOUBLE_PRECISION) ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION) SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION") diff --git a/build3/premake4.lua b/build3/premake4.lua index e1167b0aa..8fce38423 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -337,6 +337,12 @@ end trigger = "double", description = "Double precision version of Bullet" } + + newoption + { + trigger = "clamp-velocities", + description = "Limit maximum velocities to reduce FP exception risk" + } newoption { @@ -360,6 +366,9 @@ end if _OPTIONS["double"] then defines {"BT_USE_DOUBLE_PRECISION"} end + if _OPTIONS["clamp-velocities"] then + defines {"BT_CLAMP_VELOCITY_TO=9999"} + end configurations {"Release", "Debug"} configuration "Release" diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 48ecba31f..c763c75aa 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4979,22 +4979,81 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { urdfColObj.m_geometry.m_meshFileType = out_type; - if (out_type == UrdfGeometry::FILE_STL) + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH) { - CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); - glmesh = LoadMeshFromSTL(relativeFileName, fileIO); - } - if (out_type == UrdfGeometry::FILE_OBJ) - { - //create a convex hull for each shape, and store it in a btCompoundShape + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + if (out_type == UrdfGeometry::FILE_STL) + { + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + glmesh = LoadMeshFromSTL(relativeFileName, fileIO); + } - if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH) + if (out_type == UrdfGeometry::FILE_OBJ) { CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); glmesh = LoadMeshFromObj(relativeFileName, pathPrefix, fileIO); } - else + if (glmesh) { + if (compound == 0) + { + compound = worldImporter->createCompoundShape(); + } + + btTriangleMesh* meshInterface = new btTriangleMesh(); + m_data->m_meshInterfaces.push_back(meshInterface); + + for (int i = 0; i < glmesh->m_numIndices / 3; i++) + { + float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3)).xyzw; + float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 1)).xyzw; + float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 2)).xyzw; + meshInterface->addTriangle( + btVector3(v0[0], v0[1], v0[2])* meshScale, + btVector3(v1[0], v1[1], v1[2])* meshScale, + btVector3(v2[0], v2[1], v2[2])* meshScale); + } + + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true); + compound->addChildShape(childTransform, trimesh); + } + } + else + { + if (out_type == UrdfGeometry::FILE_STL) + { + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + glmesh = LoadMeshFromSTL(relativeFileName, fileIO); + + B3_PROFILE("createConvexHullFromShapes"); + if (compound == 0) + { + compound = worldImporter->createCompoundShape(); + } + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(m_data->m_defaultCollisionMargin); + for (int vv = 0; vv < glmesh->m_numvertices; vv++) + { + btVector3 pt( + glmesh->m_vertices->at(vv).xyzw[0], + glmesh->m_vertices->at(vv).xyzw[1], + glmesh->m_vertices->at(vv).xyzw[2]); + convexHull->addPoint(pt * meshScale, false); + } + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_INITIALIZE_SAT_FEATURES) + { + convexHull->initializePolyhedralFeatures(); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + + compound->addChildShape(childTransform, convexHull); + } + if (out_type == UrdfGeometry::FILE_OBJ) + { + //create a convex hull for each shape, and store it in a btCompoundShape + std::vector shapes; tinyobj::attrib_t attribute; std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); @@ -5019,19 +5078,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str { btVector3 pt; pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0], - attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], - attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], - attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], - attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], - attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], - attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); } @@ -5041,7 +5100,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str } convexHull->recalcLocalAabb(); convexHull->optimizeConvexHull(); - + compound->addChildShape(childTransform, convexHull); } } @@ -5205,6 +5264,7 @@ static void gatherVertices(const btTransform& trans, const btCollisionShape* col } default: { + printf("?\n"); } } } @@ -5214,6 +5274,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S BT_PROFILE("CMD_REQUEST_MESH_DATA"); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED; serverStatusOut.m_numDataStreamBytes = 0; + int sizeInBytes = 0; InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId); if (bodyHandle) @@ -5261,6 +5322,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { verticesOut[i] = vertices[i]; } + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex; @@ -5292,7 +5354,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S verticesOut[i] = n.m_x; } } - + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex; @@ -5301,7 +5363,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD } - serverStatusOut.m_numDataStreamBytes = 0; + serverStatusOut.m_numDataStreamBytes = sizeInBytes; return hasStatus; } @@ -5986,6 +6048,13 @@ struct BatchRayCaster int linkIndex = -1; const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + const btSoftBody* softBody = btSoftBody::upcast(rayResultCallback.m_collisionObject); + if (softBody) + { + objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); + } +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD if (body) { objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); @@ -6116,6 +6185,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); batchRayCaster.castRays(numThreads); + serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData); serverStatusOut.m_raycastHits.m_numRaycastHits = totalRays; serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; return hasStatus; @@ -6218,12 +6288,15 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar int usz = m_data->m_userConstraints.size(); int* constraintUid = bodyUids + actualNumBodies; serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; + for (int i = 0; i < usz; i++) { int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1(); constraintUid[i] = key; } + serverStatusOut.m_numDataStreamBytes = sizeof(int) * (actualNumBodies + usz); + serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; return hasStatus; } @@ -6252,14 +6325,17 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar } } } + int sizeInBytes = sizeof(int) * userDataHandles.size(); if (userDataHandles.size()) { - memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size()); + memcpy(bufferServerToClient, &userDataHandles[0], sizeInBytes); } // Only clear the client-side cache when a full sync is requested serverStatusOut.m_syncUserDataArgs.m_clearCachedUserDataEntries = clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies == 0; serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size(); + serverStatusOut.m_numDataStreamBytes = sizeInBytes; serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; + return hasStatus; } @@ -6289,6 +6365,7 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S { memcpy(bufferServerToClient, &userData->m_bytes[0], userData->m_bytes.size()); } + serverStatusOut.m_numDataStreamBytes = userData->m_bytes.size(); return hasStatus; } @@ -7638,9 +7715,18 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_linkIndexB = linkIndexB; for (int j = 0; j < 3; j++) { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + if (swap) + { + pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } } pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->m_physicsDeltaTime; @@ -8234,8 +8320,10 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe #endif } + bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) { +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btSoftBody* psb = NULL; CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); char relativeFileName[1024]; @@ -8448,6 +8536,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo psb->setTotalMass(deformable.m_mass); psb->setSelfCollision(useSelfCollision); psb->setSpringStiffness(deformable.m_repulsionStiffness); + psb->initializeFaceTree(); } #endif //SKIP_DEFORMABLE_BODY #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD @@ -8490,6 +8579,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo *bodyUniqueId = m_data->m_bodyHandles.allocHandle(); InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId); bodyHandle->m_softBody = psb; + psb->setUserIndex2(*bodyUniqueId); b3VisualShapeData visualShape; @@ -8542,9 +8632,11 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId; m_data->m_pluginManager.addNotification(notification); } +#endif return true; } + bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; @@ -8601,9 +8693,11 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; -#endif + } +#endif return hasStatus; + } bool PhysicsServerCommandProcessor::processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) @@ -10274,7 +10368,7 @@ bool PhysicsServerCommandProcessor::processRequestAabbOverlapCommand(const struc overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i]; overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i]; } - + serverCmd.m_numDataStreamBytes = numOverlap * totalBytesPerObject; serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED; //int m_startingOverlappingObjectIndex; @@ -10671,6 +10765,7 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru sharedBuf[element] = massMatrix(i, j); } } + serverCmd.m_numDataStreamBytes = sizeInBytes; serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; } } @@ -12379,6 +12474,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(cons { //extract shape info from base collider int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects); + serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes*sizeof(b3CollisionShapeData); serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes; serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED; } @@ -12388,6 +12484,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(cons if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks() && bodyHandle->m_multiBody->getLinkCollider(linkIndex)) { int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getLinkCollider(linkIndex)->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects); + serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes * sizeof(b3CollisionShapeData); serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes; serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED; } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 5c3238683..6d0ce3fa2 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -444,7 +444,12 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa break; } case UrdfGeometry::FILE_STL: - glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), fileIO); + + char relativeFileName[1024]; + if (fileIO->findResourcePath(visual->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) + { + glmesh = LoadMeshFromSTL(relativeFileName, fileIO); + } break; case UrdfGeometry::FILE_COLLADA: { diff --git a/examples/pybullet/examples/addPlanarReflection.py b/examples/pybullet/examples/addPlanarReflection.py index 6f26502a0..6cf0b4b81 100644 --- a/examples/pybullet/examples/addPlanarReflection.py +++ b/examples/pybullet/examples/addPlanarReflection.py @@ -1,6 +1,8 @@ import pybullet as p import time import math +import pybullet_data + def getRayFromTo(mouseX, mouseY): @@ -36,6 +38,7 @@ def getRayFromTo(mouseX, mouseY): cid = p.connect(p.GUI) if (cid < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") diff --git a/examples/pybullet/examples/batchRayCast.py b/examples/pybullet/examples/batchRayCast.py index ab2e3bc3c..b2de78f37 100644 --- a/examples/pybullet/examples/batchRayCast.py +++ b/examples/pybullet/examples/batchRayCast.py @@ -1,6 +1,7 @@ import pybullet as p import time import math +import pybullet_data useGui = True @@ -8,6 +9,7 @@ if (useGui): p.connect(p.GUI) else: p.connect(p.DIRECT) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) diff --git a/examples/pybullet/examples/biped2d_pybullet.py b/examples/pybullet/examples/biped2d_pybullet.py index 9b7cf687f..acf7ebeed 100644 --- a/examples/pybullet/examples/biped2d_pybullet.py +++ b/examples/pybullet/examples/biped2d_pybullet.py @@ -5,7 +5,9 @@ import time GRAVITY = -9.8 dt = 1e-3 iters = 2000 +import pybullet_data +p.setAdditionalSearchPath(pybullet_data.getDataPath()) physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() diff --git a/examples/pybullet/examples/changeDynamicsMass.py b/examples/pybullet/examples/changeDynamicsMass.py index 8e14711cd..715085ce1 100644 --- a/examples/pybullet/examples/changeDynamicsMass.py +++ b/examples/pybullet/examples/changeDynamicsMass.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) cube2 = p.loadURDF("cube.urdf", [0, 0, 3], useFixedBase=True) cube = p.loadURDF("cube.urdf", useFixedBase=True) p.setGravity(0, 0, -10) diff --git a/examples/pybullet/examples/changeTexture.py b/examples/pybullet/examples/changeTexture.py index e14ec39cf..9f1f5cf01 100644 --- a/examples/pybullet/examples/changeTexture.py +++ b/examples/pybullet/examples/changeTexture.py @@ -1,6 +1,9 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0]) planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1]) diff --git a/examples/pybullet/examples/collisionFilter.py b/examples/pybullet/examples/collisionFilter.py index e2eae0a12..13abacee9 100644 --- a/examples/pybullet/examples/collisionFilter.py +++ b/examples/pybullet/examples/collisionFilter.py @@ -1,6 +1,9 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False) cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False) diff --git a/examples/pybullet/examples/commandLogAndPlayback.py b/examples/pybullet/examples/commandLogAndPlayback.py index 94f1848cb..3ff834412 100644 --- a/examples/pybullet/examples/commandLogAndPlayback.py +++ b/examples/pybullet/examples/commandLogAndPlayback.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS, "commandLog.bin") p.loadURDF("plane.urdf") p.loadURDF("r2d2.urdf", [0, 0, 1]) diff --git a/examples/pybullet/examples/configureDebugVisualizer.py b/examples/pybullet/examples/configureDebugVisualizer.py index e1d755708..988454715 100644 --- a/examples/pybullet/examples/configureDebugVisualizer.py +++ b/examples/pybullet/examples/configureDebugVisualizer.py @@ -2,8 +2,10 @@ import pybullet as p import math import time dt = 1./240. +import pybullet_data -p.connect(p.GUI)#SHARED_MEMORY_GUI) +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("r2d2.urdf",[0,0,1]) p.loadURDF("plane.urdf") p.setGravity(0,0,-10) @@ -18,4 +20,4 @@ while (1): p.configureDebugVisualizer(lightPosition=[radius*math.sin(t),radius*math.cos(t),3]) p.stepSimulation() - time.sleep(dt) \ No newline at end of file + time.sleep(dt) diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 0d8dc0e3c..70d4a6fe9 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -2,7 +2,9 @@ import pybullet as p import time import math +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") cubeId = p.loadURDF("cube_small.urdf", 0, 0, 1) diff --git a/examples/pybullet/examples/contactFriction.py b/examples/pybullet/examples/contactFriction.py index 334e5ab71..0b113d11e 100644 --- a/examples/pybullet/examples/contactFriction.py +++ b/examples/pybullet/examples/contactFriction.py @@ -1,5 +1,8 @@ import pybullet as p +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) useMaximalCoordinates = False p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates) #p.loadURDF("sphere2.urdf",[0,0,1]) diff --git a/examples/pybullet/examples/createMesh.py b/examples/pybullet/examples/createMesh.py index 6bbb09aa3..ce7fc80d2 100644 --- a/examples/pybullet/examples/createMesh.py +++ b/examples/pybullet/examples/createMesh.py @@ -1,8 +1,10 @@ import pybullet as p import time import math +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #don't create a ground plane, to allow for gaps etc p.resetSimulation() #p.createCollisionShape(p.GEOM_PLANE) diff --git a/examples/pybullet/examples/createMultiBodyBatch.py b/examples/pybullet/examples/createMultiBodyBatch.py index dd53d99b0..87fde9c84 100644 --- a/examples/pybullet/examples/createMultiBodyBatch.py +++ b/examples/pybullet/examples/createMultiBodyBatch.py @@ -1,10 +1,11 @@ import pybullet as p import time import math - +import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setTimeStep(1. / 120.) diff --git a/examples/pybullet/examples/createMultiBodyLinks.py b/examples/pybullet/examples/createMultiBodyLinks.py index 292c887f8..cbaa6abc6 100644 --- a/examples/pybullet/examples/createMultiBodyLinks.py +++ b/examples/pybullet/examples/createMultiBodyLinks.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.createCollisionShape(p.GEOM_PLANE) p.createMultiBody(0, 0) diff --git a/examples/pybullet/examples/createObstacleCourse.py b/examples/pybullet/examples/createObstacleCourse.py index df2d37d3c..a9ab11908 100644 --- a/examples/pybullet/examples/createObstacleCourse.py +++ b/examples/pybullet/examples/createObstacleCourse.py @@ -1,8 +1,10 @@ import pybullet as p import time import math +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #don't create a ground plane, to allow for gaps etc p.resetSimulation() #p.createCollisionShape(p.GEOM_PLANE) diff --git a/examples/pybullet/examples/createSphereMultiBodies.py b/examples/pybullet/examples/createSphereMultiBodies.py index 689205ca3..67f562d70 100644 --- a/examples/pybullet/examples/createSphereMultiBodies.py +++ b/examples/pybullet/examples/createSphereMultiBodies.py @@ -2,8 +2,10 @@ import pybullet as p import time useMaximalCoordinates = 0 +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) monastryId = concaveEnv = p.createCollisionShape(p.GEOM_MESH, fileName="samurai_monastry.obj", diff --git a/examples/pybullet/examples/createTexturedMeshVisualShape.py b/examples/pybullet/examples/createTexturedMeshVisualShape.py index b84933741..00e2b6252 100644 --- a/examples/pybullet/examples/createTexturedMeshVisualShape.py +++ b/examples/pybullet/examples/createTexturedMeshVisualShape.py @@ -1,6 +1,7 @@ import pybullet as p import time import math +import pybullet_data def getRayFromTo(mouseX, mouseY): @@ -35,6 +36,7 @@ def getRayFromTo(mouseX, mouseY): cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") diff --git a/examples/pybullet/examples/createVisualShape.py b/examples/pybullet/examples/createVisualShape.py index 9407feba3..26506ca96 100644 --- a/examples/pybullet/examples/createVisualShape.py +++ b/examples/pybullet/examples/createVisualShape.py @@ -1,6 +1,7 @@ import pybullet as p import time import math +import pybullet_data def getRayFromTo(mouseX, mouseY): @@ -36,6 +37,8 @@ def getRayFromTo(mouseX, mouseY): cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") diff --git a/examples/pybullet/examples/createVisualShapeArray.py b/examples/pybullet/examples/createVisualShapeArray.py index 713738eb1..6156d808c 100644 --- a/examples/pybullet/examples/createVisualShapeArray.py +++ b/examples/pybullet/examples/createVisualShapeArray.py @@ -1,6 +1,7 @@ import pybullet as p import time import math +import pybullet_data def getRayFromTo(mouseX, mouseY): @@ -35,6 +36,8 @@ def getRayFromTo(mouseX, mouseY): cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") diff --git a/examples/pybullet/examples/debugDrawItems.py b/examples/pybullet/examples/debugDrawItems.py index 0aac654bc..3b2137445 100644 --- a/examples/pybullet/examples/debugDrawItems.py +++ b/examples/pybullet/examples/debugDrawItems.py @@ -1,9 +1,12 @@ import pybullet as p import time +import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") kuka = p.loadURDF("kuka_iiwa/model.urdf") p.addUserDebugText("tip", [0, 0, 0.1], diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py index 666334628..1c01ed6d4 100644 --- a/examples/pybullet/examples/deformable_anchor.py +++ b/examples/pybullet/examples/deformable_anchor.py @@ -2,6 +2,9 @@ import pybullet as p from time import sleep physicsClient = p.connect(p.GUI) +import pybullet_data + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) gravZ=-10 diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py index 279737fa4..ffda4e6b7 100644 --- a/examples/pybullet/examples/deformable_ball.py +++ b/examples/pybullet/examples/deformable_ball.py @@ -1,7 +1,9 @@ import pybullet as p from time import sleep +import pybullet_data physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index 8416b651c..7e2732fb3 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -1,8 +1,11 @@ import pybullet as p from time import sleep +import pybullet_data physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) p.setGravity(0, 0, -10) diff --git a/examples/pybullet/examples/eglRenderTest.py b/examples/pybullet/examples/eglRenderTest.py index 13dbf2091..916467c2d 100644 --- a/examples/pybullet/examples/eglRenderTest.py +++ b/examples/pybullet/examples/eglRenderTest.py @@ -2,9 +2,10 @@ import pybullet as p import time import pkgutil egl = pkgutil.get_loader('eglRenderer') +import pybullet_data p.connect(p.DIRECT) - +p.setAdditionalSearchPath(pybullet_data.getDataPath()) plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin") print("plugin=", plugin) diff --git a/examples/pybullet/examples/experimentalCcdSphereRadius.py b/examples/pybullet/examples/experimentalCcdSphereRadius.py index 6f3604b1b..ca7dd664a 100644 --- a/examples/pybullet/examples/experimentalCcdSphereRadius.py +++ b/examples/pybullet/examples/experimentalCcdSphereRadius.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(allowedCcdPenetration=0.0) terrain_mass = 0 diff --git a/examples/pybullet/examples/externalTorqueControlledSphere.py b/examples/pybullet/examples/externalTorqueControlledSphere.py index 45b2cfe70..544811667 100644 --- a/examples/pybullet/examples/externalTorqueControlledSphere.py +++ b/examples/pybullet/examples/externalTorqueControlledSphere.py @@ -1,9 +1,11 @@ import pybullet as p import pybullet_data import time +import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadSDF("stadium.sdf") p.setGravity(0, 0, -10) objects = p.loadMJCF("mjcf/sphere.xml") diff --git a/examples/pybullet/examples/fileIOPlugin.py b/examples/pybullet/examples/fileIOPlugin.py index d4a9ce606..9b3c2b546 100644 --- a/examples/pybullet/examples/fileIOPlugin.py +++ b/examples/pybullet/examples/fileIOPlugin.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) fileIO = p.loadPlugin("fileIOPlugin") if (fileIO >= 0): #we can have a zipfile (pickup.zip) inside a zipfile (pickup2.zip) diff --git a/examples/pybullet/examples/forcetorquesensor.py b/examples/pybullet/examples/forcetorquesensor.py index 5293ab1e1..5b7e3987e 100644 --- a/examples/pybullet/examples/forcetorquesensor.py +++ b/examples/pybullet/examples/forcetorquesensor.py @@ -1,5 +1,8 @@ import pybullet as p -p.connect(p.DIRECT) +import pybullet_data + +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) hinge = p.loadURDF("hinge.urdf") print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg") diff --git a/examples/pybullet/examples/frictionCone.py b/examples/pybullet/examples/frictionCone.py index f12a6abd3..b5d60ecfb 100644 --- a/examples/pybullet/examples/frictionCone.py +++ b/examples/pybullet/examples/frictionCone.py @@ -2,7 +2,10 @@ import pybullet as p import time import math +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) useMaximalCoordinates = False p.setGravity(0, 0, -10) diff --git a/examples/pybullet/examples/getAABB.py b/examples/pybullet/examples/getAABB.py index b80a6fe6d..a94e581f0 100644 --- a/examples/pybullet/examples/getAABB.py +++ b/examples/pybullet/examples/getAABB.py @@ -1,4 +1,5 @@ import pybullet as p +import pybullet_data draw = 1 printtext = 0 @@ -6,6 +7,8 @@ if (draw): p.connect(p.GUI) else: p.connect(p.DIRECT) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) r2d2 = p.loadURDF("r2d2.urdf") diff --git a/examples/pybullet/examples/getCameraImageTest.py b/examples/pybullet/examples/getCameraImageTest.py index c68888577..ceae99566 100644 --- a/examples/pybullet/examples/getCameraImageTest.py +++ b/examples/pybullet/examples/getCameraImageTest.py @@ -2,10 +2,12 @@ import matplotlib.pyplot as plt import numpy as np import pybullet as p import time +import pybullet_data direct = p.connect(p.GUI) #, options="--window_backend=2 --render_device=0") #egl = p.loadPlugin("eglRendererPlugin") +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF('plane.urdf') p.loadURDF("r2d2.urdf", [0, 0, 1]) p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025]) diff --git a/examples/pybullet/examples/getClosestPoints.py b/examples/pybullet/examples/getClosestPoints.py index c8db4dfb0..55311f58d 100644 --- a/examples/pybullet/examples/getClosestPoints.py +++ b/examples/pybullet/examples/getClosestPoints.py @@ -1,6 +1,9 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) useCollisionShapeQuery = True p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1) diff --git a/examples/pybullet/examples/getTextureUid.py b/examples/pybullet/examples/getTextureUid.py index e8fbe1462..f4a31b2f0 100644 --- a/examples/pybullet/examples/getTextureUid.py +++ b/examples/pybullet/examples/getTextureUid.py @@ -1,5 +1,8 @@ import pybullet as p +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) plane = p.loadURDF("plane.urdf") visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) print(visualData) diff --git a/examples/pybullet/examples/grpcClient.py b/examples/pybullet/examples/grpcClient.py index e28e1112a..98b2ba853 100644 --- a/examples/pybullet/examples/grpcClient.py +++ b/examples/pybullet/examples/grpcClient.py @@ -1,4 +1,6 @@ import pybullet as p +import pybullet_data + usePort = True @@ -13,5 +15,7 @@ if (id < 0): exit(0) print("Connected to GRPC") + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) r2d2 = p.loadURDF("r2d2.urdf") print("numJoints = ", p.getNumJoints(r2d2)) diff --git a/examples/pybullet/examples/grpcServer.py b/examples/pybullet/examples/grpcServer.py index 7b4e62485..24385706e 100644 --- a/examples/pybullet/examples/grpcServer.py +++ b/examples/pybullet/examples/grpcServer.py @@ -4,7 +4,10 @@ import time useDirect = False usePort = True +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) id = p.loadPlugin("grpcPlugin") #dynamically loading the plugin #id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin") diff --git a/examples/pybullet/examples/hand.py b/examples/pybullet/examples/hand.py index 5f1b0793a..165a9c193 100644 --- a/examples/pybullet/examples/hand.py +++ b/examples/pybullet/examples/hand.py @@ -8,6 +8,8 @@ import serial import time import pybullet as p +import pybullet_data + #first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) @@ -15,6 +17,8 @@ print(c) if (c < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + #load the MuJoCo MJCF hand objects = p.loadMJCF("MPL/MPL.xml") diff --git a/examples/pybullet/examples/hello_pybullet.py b/examples/pybullet/examples/hello_pybullet.py index 389f2dc67..fdca83cce 100644 --- a/examples/pybullet/examples/hello_pybullet.py +++ b/examples/pybullet/examples/hello_pybullet.py @@ -1,8 +1,12 @@ import pybullet as p from time import sleep +import pybullet_data + physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") cubeStartPos = [0, 0, 1] diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index f810f533e..ac0afda48 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -1,12 +1,15 @@ import pybullet as p import json import time +import pybullet_data + useGUI = True if useGUI: p.connect(p.GUI) else: p.connect(p.DIRECT) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) useZUp = False useYUp = not useZUp diff --git a/examples/pybullet/examples/humanoid_benchmark.py b/examples/pybullet/examples/humanoid_benchmark.py index c063f9006..6666f2c03 100644 --- a/examples/pybullet/examples/humanoid_benchmark.py +++ b/examples/pybullet/examples/humanoid_benchmark.py @@ -1,6 +1,9 @@ import pybullet as p import time +import pybullet_data + p.connect(p.DIRECT) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(numSolverIterations=5) p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.) diff --git a/examples/pybullet/examples/humanoid_knee_position_control.py b/examples/pybullet/examples/humanoid_knee_position_control.py index f43a7cb1e..17188c6d2 100644 --- a/examples/pybullet/examples/humanoid_knee_position_control.py +++ b/examples/pybullet/examples/humanoid_knee_position_control.py @@ -1,5 +1,8 @@ import pybullet as p import time +import pybullet_data + + cid = p.connect(p.SHARED_MEMORY) if (cid < 0): @@ -11,6 +14,8 @@ useRealTime = 0 p.setRealTimeSimulation(useRealTime) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.setGravity(0, 0, -10) p.loadSDF("stadium.sdf") diff --git a/examples/pybullet/examples/humanoid_manual_control.py b/examples/pybullet/examples/humanoid_manual_control.py index e6170f43a..891cfab6a 100644 --- a/examples/pybullet/examples/humanoid_manual_control.py +++ b/examples/pybullet/examples/humanoid_manual_control.py @@ -1,7 +1,10 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) obUids = p.loadMJCF("mjcf/humanoid.xml") humanoid = obUids[1] diff --git a/examples/pybullet/examples/ik_end_effector_orientation.py b/examples/pybullet/examples/ik_end_effector_orientation.py index 5163fbad3..2c9cf3403 100644 --- a/examples/pybullet/examples/ik_end_effector_orientation.py +++ b/examples/pybullet/examples/ik_end_effector_orientation.py @@ -4,7 +4,10 @@ import math from datetime import datetime from time import sleep +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) diff --git a/examples/pybullet/examples/integrate.py b/examples/pybullet/examples/integrate.py index a706dd00e..f711e09f9 100644 --- a/examples/pybullet/examples/integrate.py +++ b/examples/pybullet/examples/integrate.py @@ -1,5 +1,8 @@ import pybullet as p +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) cube = p.loadURDF("cube.urdf") frequency = 240 timeStep = 1. / frequency diff --git a/examples/pybullet/examples/internalEdge.py b/examples/pybullet/examples/internalEdge.py index fa8dbcb9b..7cd303b9f 100644 --- a/examples/pybullet/examples/internalEdge.py +++ b/examples/pybullet/examples/internalEdge.py @@ -1,7 +1,10 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) if (1): box_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX, diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index 11ec4ed4f..bf198ff32 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -2,12 +2,15 @@ import pybullet as p import time import math from datetime import datetime +import pybullet_data clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.GUI) #p.connect(p.SHARED_MEMORY_GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.loadURDF("plane.urdf", [0, 0, -0.3]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) diff --git a/examples/pybullet/examples/inverse_kinematics_husky_kuka.py b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py index 7b3bb793d..256d57ae6 100644 --- a/examples/pybullet/examples/inverse_kinematics_husky_kuka.py +++ b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py @@ -3,8 +3,11 @@ import time import math from datetime import datetime from datetime import datetime +import pybullet_data clid = p.connect(p.SHARED_MEMORY) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) if (clid < 0): p.connect(p.GUI) p.loadURDF("plane.urdf", [0, 0, -0.3]) diff --git a/examples/pybullet/examples/inverse_kinematics_pole.py b/examples/pybullet/examples/inverse_kinematics_pole.py index fec0dbaf6..653581a0e 100755 --- a/examples/pybullet/examples/inverse_kinematics_pole.py +++ b/examples/pybullet/examples/inverse_kinematics_pole.py @@ -2,10 +2,13 @@ import pybullet as p import time import math from datetime import datetime +import pybullet_data clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", [0, 0, -1.3]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) sawyerId = p.loadURDF("pole.urdf", [0, 0, 0]) diff --git a/examples/pybullet/examples/jacobian.py b/examples/pybullet/examples/jacobian.py index 3cdb4752e..4a7e9725c 100644 --- a/examples/pybullet/examples/jacobian.py +++ b/examples/pybullet/examples/jacobian.py @@ -1,4 +1,6 @@ import pybullet as p +import pybullet_data + def getJointStates(robot): @@ -51,6 +53,8 @@ clid = p.connect(p.SHARED_MEMORY) if (clid < 0): p.connect(p.DIRECT) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + time_step = 0.001 gravity_constant = -9.81 p.resetSimulation() diff --git a/examples/pybullet/examples/jointFrictionAndMotor.py b/examples/pybullet/examples/jointFrictionAndMotor.py index 028fac915..404adf63e 100644 --- a/examples/pybullet/examples/jointFrictionAndMotor.py +++ b/examples/pybullet/examples/jointFrictionAndMotor.py @@ -1,7 +1,10 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) door = p.loadURDF("door.urdf") #linear/angular damping for base and all children=0 diff --git a/examples/pybullet/examples/jointFrictionDamping.py b/examples/pybullet/examples/jointFrictionDamping.py index a69c021d8..b1d7bfded 100644 --- a/examples/pybullet/examples/jointFrictionDamping.py +++ b/examples/pybullet/examples/jointFrictionDamping.py @@ -1,7 +1,10 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", [0, 0, -0.25]) minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True) print(p.getNumJoints(minitaur)) diff --git a/examples/pybullet/examples/kuka_grasp_block_playback.py b/examples/pybullet/examples/kuka_grasp_block_playback.py index 1d0fbeb8c..1aa3f755d 100644 --- a/examples/pybullet/examples/kuka_grasp_block_playback.py +++ b/examples/pybullet/examples/kuka_grasp_block_playback.py @@ -43,7 +43,10 @@ def readLogFile(filename, verbose=True): #clid = p.connect(p.SHARED_MEMORY) +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") p.loadURDF("tray/tray.urdf", [0, 0, 0]) p.loadURDF("block.urdf", [0, 0, 2]) diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index 49573ea83..e06fd1487 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/examples/pybullet/examples/kuka_with_cube.py @@ -4,7 +4,10 @@ import math from datetime import datetime #clid = p.connect(p.SHARED_MEMORY) +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", [0, 0, -0.3], useFixedBase=True) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1]) diff --git a/examples/pybullet/examples/kuka_with_cube_playback.py b/examples/pybullet/examples/kuka_with_cube_playback.py index 1a79b42d1..0345b0531 100644 --- a/examples/pybullet/examples/kuka_with_cube_playback.py +++ b/examples/pybullet/examples/kuka_with_cube_playback.py @@ -52,7 +52,10 @@ def readLogFile(filename, verbose=True): #clid = p.connect(p.SHARED_MEMORY) +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", [0, 0, -0.3]) p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 1]) p.loadURDF("cube.urdf", [2, 2, 5]) diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py index 3c55ca2e6..04cccfecc 100644 --- a/examples/pybullet/examples/load_soft_body.py +++ b/examples/pybullet/examples/load_soft_body.py @@ -1,7 +1,11 @@ import pybullet as p from time import sleep +import pybullet_data + physicsClient = p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) diff --git a/examples/pybullet/examples/loadingBench.py b/examples/pybullet/examples/loadingBench.py index fcd886df5..672adbedc 100644 --- a/examples/pybullet/examples/loadingBench.py +++ b/examples/pybullet/examples/loadingBench.py @@ -1,6 +1,9 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json") diff --git a/examples/pybullet/examples/logMinitaur.py b/examples/pybullet/examples/logMinitaur.py index 8338862f7..c8418b9ec 100644 --- a/examples/pybullet/examples/logMinitaur.py +++ b/examples/pybullet/examples/logMinitaur.py @@ -1,7 +1,12 @@ import pybullet as p +import pybullet_data + + cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf") diff --git a/examples/pybullet/examples/manyspheres.py b/examples/pybullet/examples/manyspheres.py index 55af537a8..4c6e5197c 100644 --- a/examples/pybullet/examples/manyspheres.py +++ b/examples/pybullet/examples/manyspheres.py @@ -1,10 +1,13 @@ import pybullet as p import time +import pybullet_data + conid = p.connect(p.SHARED_MEMORY) if (conid < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setInternalSimFlags(0) p.resetSimulation() diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py index e6c42d6a2..7a54f5c10 100644 --- a/examples/pybullet/examples/mimicJointConstraint.py +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -3,7 +3,10 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", 0, 0, -2) wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0]) for i in range(p.getNumJoints(wheelA)): diff --git a/examples/pybullet/examples/minitaur_test.py b/examples/pybullet/examples/minitaur_test.py index 37c2ff37b..6658e204b 100644 --- a/examples/pybullet/examples/minitaur_test.py +++ b/examples/pybullet/examples/minitaur_test.py @@ -9,6 +9,9 @@ from minitaur_evaluate import * import time import math import numpy as np +import pybullet_data + + def main(unused_args): @@ -16,7 +19,7 @@ def main(unused_args): c = p.connect(p.SHARED_MEMORY) if (c < 0): c = p.connect(p.GUI) - + p.setAdditionalSearchPath(pybullet_data.getDataPath()) params = [ 0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539 diff --git a/examples/pybullet/examples/motorMaxVelocity.py b/examples/pybullet/examples/motorMaxVelocity.py index 315e30d90..95a0c1e39 100644 --- a/examples/pybullet/examples/motorMaxVelocity.py +++ b/examples/pybullet/examples/motorMaxVelocity.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) cartpole = p.loadURDF("cartpole.urdf") p.setRealTimeSimulation(1) p.setJointMotorControl2(cartpole, diff --git a/examples/pybullet/examples/otherPhysicsEngine.py b/examples/pybullet/examples/otherPhysicsEngine.py index e12bb45d4..59e0f18d1 100644 --- a/examples/pybullet/examples/otherPhysicsEngine.py +++ b/examples/pybullet/examples/otherPhysicsEngine.py @@ -2,6 +2,7 @@ import pybullet as p import pybullet_data as pd import time import math +import pybullet_data usePhysX = True useMaximalCoordinates = True @@ -11,6 +12,7 @@ if usePhysX: else: p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(fixedTimeStep=1. / 240., numSolverIterations=4, minimumSolverIslandSize=1024) diff --git a/examples/pybullet/examples/pdControl.py b/examples/pybullet/examples/pdControl.py index ef0718dfc..e9c902e23 100644 --- a/examples/pybullet/examples/pdControl.py +++ b/examples/pybullet/examples/pdControl.py @@ -6,7 +6,10 @@ from pdControllerStable import PDControllerStable import time useMaximalCoordinates = False +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates) pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates) pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates) diff --git a/examples/pybullet/examples/pointCloudFromCameraImage.py b/examples/pybullet/examples/pointCloudFromCameraImage.py index e7aa33fad..a156090f8 100644 --- a/examples/pybullet/examples/pointCloudFromCameraImage.py +++ b/examples/pybullet/examples/pointCloudFromCameraImage.py @@ -1,8 +1,10 @@ import pybullet as p import math import numpy as np +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) plane = p.loadURDF("plane100.urdf") cube = p.loadURDF("cube.urdf", [0, 0, 1]) diff --git a/examples/pybullet/examples/profileTiming.py b/examples/pybullet/examples/profileTiming.py index 0904a8f52..96c25b300 100644 --- a/examples/pybullet/examples/profileTiming.py +++ b/examples/pybullet/examples/profileTiming.py @@ -2,7 +2,10 @@ import pybullet as p import time #you can visualize the timings using Google Chrome, visit about://tracing #and load the json file +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) t = time.time() + 3.1 diff --git a/examples/pybullet/examples/projective_texture.py b/examples/pybullet/examples/projective_texture.py index 1f38eb000..6116c59b0 100644 --- a/examples/pybullet/examples/projective_texture.py +++ b/examples/pybullet/examples/projective_texture.py @@ -2,9 +2,11 @@ import pybullet as p from time import sleep import matplotlib.pyplot as plt import numpy as np +import pybullet_data physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, 0) bearStartPos1 = [-3.3, 0, 0] bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0]) diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 6d72abb86..356bb777f 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -1,6 +1,9 @@ import pybullet as p import time import math +import pybullet_data + + def drawInertiaBox(parentUid, parentLinkIndex, color): @@ -128,6 +131,7 @@ if (physId < 0): p.connect(p.GUI) #p.resetSimulation() +p.setAdditionalSearchPath(pybullet_data.getDataPath()) angle = 0 # pick in range 0..0.2 radians orn = p.getQuaternionFromEuler([0, angle, 0]) p.loadURDF("plane.urdf", [0, 0, 0], orn) diff --git a/examples/pybullet/examples/quadruped_playback.py b/examples/pybullet/examples/quadruped_playback.py index 544009745..626f29a5f 100644 --- a/examples/pybullet/examples/quadruped_playback.py +++ b/examples/pybullet/examples/quadruped_playback.py @@ -9,6 +9,7 @@ import sys import os, fnmatch import argparse from time import sleep +import pybullet_data def readLogFile(filename, verbose=True): @@ -56,6 +57,7 @@ def readLogFile(filename, verbose=True): clid = p.connect(p.SHARED_MEMORY) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) log = readLogFile("LOG00076.TXT") recordNum = len(log) diff --git a/examples/pybullet/examples/quadruped_setup_playback.py b/examples/pybullet/examples/quadruped_setup_playback.py index 5db18cb07..f71ff4f19 100644 --- a/examples/pybullet/examples/quadruped_setup_playback.py +++ b/examples/pybullet/examples/quadruped_setup_playback.py @@ -1,5 +1,9 @@ import pybullet as p +import pybullet_data + p.connect(p.SHARED_MEMORY) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) objects = [ p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000) ] diff --git a/examples/pybullet/examples/racecar_differential.py b/examples/pybullet/examples/racecar_differential.py index 1967f3277..abd2b904f 100644 --- a/examples/pybullet/examples/racecar_differential.py +++ b/examples/pybullet/examples/racecar_differential.py @@ -1,10 +1,13 @@ import pybullet as p import time +import pybullet_data + cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() p.setGravity(0, 0, -10) useRealTimeSim = 1 diff --git a/examples/pybullet/examples/renderPlugin.py b/examples/pybullet/examples/renderPlugin.py index 1f7a1ebfb..111aef5b3 100644 --- a/examples/pybullet/examples/renderPlugin.py +++ b/examples/pybullet/examples/renderPlugin.py @@ -1,5 +1,8 @@ import pybullet as p +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll", "_testPlugin") print("plugin=", plugin) diff --git a/examples/pybullet/examples/rendertest.py b/examples/pybullet/examples/rendertest.py index 6e3d14b89..e3fcd2fca 100644 --- a/examples/pybullet/examples/rendertest.py +++ b/examples/pybullet/examples/rendertest.py @@ -10,6 +10,8 @@ import subprocess import numpy as np import pybullet from multiprocessing import Process +import pybullet_data + camTargetPos = [0, 0, 0] cameraUp = [0, 0, 1] @@ -41,6 +43,7 @@ class BulletSim(): print(self.connection_mode, optionstring, *self.argv) cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv) + pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) if cid < 0: raise ValueError print("connected") diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py index 1092496f8..d42aa558f 100644 --- a/examples/pybullet/examples/reset_dynamic_info.py +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -2,7 +2,10 @@ import pybullet as p import time import math +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) planeId = p.loadURDF(fileName="plane.urdf", baseOrientation=[0.25882, 0, 0, 0.96593]) p.loadURDF(fileName="cube.urdf", basePosition=[0, 0, 2]) cubeId = p.loadURDF(fileName="cube.urdf", baseOrientation=[0, 0, 0, 1], basePosition=[0, 0, 4]) diff --git a/examples/pybullet/examples/restitution.py b/examples/pybullet/examples/restitution.py index dc169fe48..5e84b3b61 100644 --- a/examples/pybullet/examples/restitution.py +++ b/examples/pybullet/examples/restitution.py @@ -3,10 +3,13 @@ import pybullet as p import time +import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): cid = p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) restitutionId = p.addUserDebugParameter("restitution", 0, 1, 1) restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold", 0, 3, 0.2) diff --git a/examples/pybullet/examples/robotcontrol.py b/examples/pybullet/examples/robotcontrol.py index 7633baf18..9bc917c30 100644 --- a/examples/pybullet/examples/robotcontrol.py +++ b/examples/pybullet/examples/robotcontrol.py @@ -1,5 +1,8 @@ import pybullet as p -p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT +import pybullet_data + +p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") p.setGravity(0, 0, -10) diff --git a/examples/pybullet/examples/rollPitchYaw.py b/examples/pybullet/examples/rollPitchYaw.py index 5ab2793ae..e5dac10ba 100644 --- a/examples/pybullet/examples/rollPitchYaw.py +++ b/examples/pybullet/examples/rollPitchYaw.py @@ -1,9 +1,12 @@ import pybullet as p import time +import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True) rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0) pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0) diff --git a/examples/pybullet/examples/rotationalFriction.py b/examples/pybullet/examples/rotationalFriction.py index f38cba38c..b8f524cb2 100644 --- a/examples/pybullet/examples/rotationalFriction.py +++ b/examples/pybullet/examples/rotationalFriction.py @@ -3,7 +3,10 @@ import pybullet as p import pybullet_data as pd useMaximalCoordinatesA = True useMaximalCoordinatesB = True +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pd.getDataPath()) cube=p.loadURDF("cube_rotate.urdf",useMaximalCoordinates=useMaximalCoordinatesA) p.loadURDF("sphere2.urdf",[0,0,2],useMaximalCoordinates=useMaximalCoordinatesB) diff --git a/examples/pybullet/examples/satCollision.py b/examples/pybullet/examples/satCollision.py index c1da8d1b4..af68e143f 100644 --- a/examples/pybullet/examples/satCollision.py +++ b/examples/pybullet/examples/satCollision.py @@ -1,7 +1,10 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(enableSAT=1) p.loadURDF("cube_concave.urdf", [0, 0, -25], diff --git a/examples/pybullet/examples/saveRestoreState.py b/examples/pybullet/examples/saveRestoreState.py index 7bce8313f..f9e12d4a6 100644 --- a/examples/pybullet/examples/saveRestoreState.py +++ b/examples/pybullet/examples/saveRestoreState.py @@ -1,6 +1,8 @@ import pybullet as p import math, time import difflib, sys +import pybullet_data + numSteps = 500 numSteps2 = 30 @@ -8,6 +10,7 @@ p.connect(p.GUI, options="--width=1024 --height=768") numObjects = 50 verbose = 0 +p.setAdditionalSearchPath(pybullet_data.getDataPath()) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log") diff --git a/examples/pybullet/examples/saveWorld.py b/examples/pybullet/examples/saveWorld.py index f6f6278ed..e4026770f 100644 --- a/examples/pybullet/examples/saveWorld.py +++ b/examples/pybullet/examples/saveWorld.py @@ -1,7 +1,10 @@ import pybullet as p import time +import pybullet_data p.connect(p.SHARED_MEMORY) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) timestr = time.strftime("%Y%m%d-%H%M%S") filename = "saveWorld" + timestr + ".py" p.saveWorld(filename) diff --git a/examples/pybullet/examples/sceneAabb.py b/examples/pybullet/examples/sceneAabb.py index aa11ff288..260fa4f64 100644 --- a/examples/pybullet/examples/sceneAabb.py +++ b/examples/pybullet/examples/sceneAabb.py @@ -2,7 +2,10 @@ import pybullet as p import time import numpy as np +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") p.loadURDF("sphere2.urdf",[0,0,2]) diff --git a/examples/pybullet/examples/segmask_linkindex.py b/examples/pybullet/examples/segmask_linkindex.py index a32fb037f..6733ce3e1 100644 --- a/examples/pybullet/examples/segmask_linkindex.py +++ b/examples/pybullet/examples/segmask_linkindex.py @@ -1,5 +1,8 @@ import pybullet as p +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1]) for l in range(p.getNumJoints(r2d2)): print(p.getJointInfo(r2d2, l)) diff --git a/examples/pybullet/examples/shiftCenterOfMass.py b/examples/pybullet/examples/shiftCenterOfMass.py index c0a3f1911..95a507d91 100644 --- a/examples/pybullet/examples/shiftCenterOfMass.py +++ b/examples/pybullet/examples/shiftCenterOfMass.py @@ -2,8 +2,11 @@ import pybullet as p import time import pybullet_data +import pybullet_data + p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") p.setGravity(0, 0, -10) diff --git a/examples/pybullet/examples/signedDistanceField.py b/examples/pybullet/examples/signedDistanceField.py index c3aac70f9..e1f94fc00 100644 --- a/examples/pybullet/examples/signedDistanceField.py +++ b/examples/pybullet/examples/signedDistanceField.py @@ -1,7 +1,10 @@ import pybullet as p import pybullet import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("toys/concave_box.urdf") p.setGravity(0, 0, -10) for i in range(10): diff --git a/examples/pybullet/examples/sleeping.py b/examples/pybullet/examples/sleeping.py index e9ca7f404..fa4ef72c1 100644 --- a/examples/pybullet/examples/sleeping.py +++ b/examples/pybullet/examples/sleeping.py @@ -4,7 +4,10 @@ useMaximalCoordinates = False flags = p.URDF_ENABLE_SLEEPING +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.loadURDF("plane100.urdf", flags=flags, useMaximalCoordinates=useMaximalCoordinates) diff --git a/examples/pybullet/examples/snake.py b/examples/pybullet/examples/snake.py index 1384a3f53..d70a50e0a 100644 --- a/examples/pybullet/examples/snake.py +++ b/examples/pybullet/examples/snake.py @@ -4,8 +4,10 @@ import math # This simple snake logic is from some 15 year old Havok C++ demo # Thanks to Michael Ewert! +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) plane = p.createCollisionShape(p.GEOM_PLANE) p.createMultiBody(0, plane) diff --git a/examples/pybullet/examples/switchConstraintSolver.py b/examples/pybullet/examples/switchConstraintSolver.py index 1c44391f7..b526789cc 100644 --- a/examples/pybullet/examples/switchConstraintSolver.py +++ b/examples/pybullet/examples/switchConstraintSolver.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM=0.000001) diff --git a/examples/pybullet/examples/testPlugin.py b/examples/pybullet/examples/testPlugin.py index c3abe25ff..ed1715c23 100644 --- a/examples/pybullet/examples/testPlugin.py +++ b/examples/pybullet/examples/testPlugin.py @@ -1,5 +1,8 @@ import pybullet as p +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) plugin = p.loadPlugin("D:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll", "_testPlugin") print("plugin=", plugin) diff --git a/examples/pybullet/examples/test_inertia.py b/examples/pybullet/examples/test_inertia.py index a290f3bac..d30827db7 100644 --- a/examples/pybullet/examples/test_inertia.py +++ b/examples/pybullet/examples/test_inertia.py @@ -1,8 +1,11 @@ import pybullet as p from time import sleep +import pybullet_data + physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) useDeformable = True if useDeformable: p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py index 0332f2a6c..cbcda6e5b 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -11,9 +11,12 @@ img = [[1, 2, 3] * 50] * 100 #np.random.rand(200, 320) #img = [tandard_normal((50,100)) image = plt.imshow(img, interpolation='none', animated=True, label="blah") ax = plt.gca() +import pybullet_data pybullet.connect(pybullet.DIRECT) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + #pybullet.loadPlugin("eglRendererPlugin") pybullet.loadURDF("plane.urdf", [0, 0, -1]) pybullet.loadURDF("r2d2.urdf") diff --git a/examples/pybullet/examples/testrender_egl.py b/examples/pybullet/examples/testrender_egl.py index 3c9727b1a..6aca0eca8 100644 --- a/examples/pybullet/examples/testrender_egl.py +++ b/examples/pybullet/examples/testrender_egl.py @@ -17,9 +17,13 @@ img = np.random.rand(200, 320) #img = [tandard_normal((50,100)) image = plt.imshow(img, interpolation='none', animated=True, label="blah") ax = plt.gca() +import pybullet_data + pybullet.connect(pybullet.DIRECT) +pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) + egl = pkgutil.get_loader('eglRenderer') if (egl): pybullet.loadPlugin(egl.get_filename(), "_eglRendererPlugin") diff --git a/examples/pybullet/examples/testrender_np.py b/examples/pybullet/examples/testrender_np.py index d32edd09e..2c313a457 100644 --- a/examples/pybullet/examples/testrender_np.py +++ b/examples/pybullet/examples/testrender_np.py @@ -6,6 +6,7 @@ import numpy as np import matplotlib.pyplot as plt import pybullet import time +import pybullet_data plt.ion() @@ -16,6 +17,8 @@ ax = plt.gca() #pybullet.connect(pybullet.GUI) pybullet.connect(pybullet.DIRECT) + +pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) pybullet.loadURDF("plane.urdf", [0, 0, -1]) pybullet.loadURDF("r2d2.urdf") diff --git a/examples/pybullet/examples/transparent.py b/examples/pybullet/examples/transparent.py index 92a95d44a..8a4831002 100644 --- a/examples/pybullet/examples/transparent.py +++ b/examples/pybullet/examples/transparent.py @@ -1,6 +1,9 @@ import pybullet as p import time +import pybullet_data + p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") sphereUid = p.loadURDF("sphere_transparent.urdf", [0, 0, 2]) diff --git a/examples/pybullet/examples/urdfEditor.py b/examples/pybullet/examples/urdfEditor.py index 0b5404318..9d29dd010 100644 --- a/examples/pybullet/examples/urdfEditor.py +++ b/examples/pybullet/examples/urdfEditor.py @@ -1,6 +1,8 @@ import pybullet as p import time from pybullet_utils import urdfEditor +import pybullet_data + ########################################## org2 = p.connect(p.DIRECT) @@ -10,6 +12,8 @@ if (org < 0): gui = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.resetSimulation(physicsClientId=org) #door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf diff --git a/examples/pybullet/examples/vhacd.py b/examples/pybullet/examples/vhacd.py index 544590d98..8b4bcf77d 100644 --- a/examples/pybullet/examples/vhacd.py +++ b/examples/pybullet/examples/vhacd.py @@ -2,7 +2,10 @@ import pybullet as p import pybullet_data as pd import os +import pybullet_data + p.connect(p.DIRECT) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) name_in = os.path.join(pd.getDataPath(), "duck.obj") name_out = "duck_vhacd.obj" name_log = "log.txt" diff --git a/examples/pybullet/examples/video_sync_mp4.py b/examples/pybullet/examples/video_sync_mp4.py index ba8956bec..3859de538 100644 --- a/examples/pybullet/examples/video_sync_mp4.py +++ b/examples/pybullet/examples/video_sync_mp4.py @@ -1,8 +1,11 @@ import pybullet as p import time +import pybullet_data #by default, PyBullet runs at 240Hz p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240") + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1) p.loadURDF("plane.urdf") diff --git a/examples/pybullet/examples/vrEvent.py b/examples/pybullet/examples/vrEvent.py index 562b37cc0..068b02552 100644 --- a/examples/pybullet/examples/vrEvent.py +++ b/examples/pybullet/examples/vrEvent.py @@ -4,6 +4,7 @@ # Line width can be changed import pybullet as p +import pybullet_data CONTROLLER_ID = 0 POSITION = 1 @@ -13,11 +14,14 @@ BUTTONS = 6 ANALOG_AXIS = 8 #assume that the VR physics server is already started before + c = p.connect(p.SHARED_MEMORY) print(c) if (c < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.setInternalSimFlags(0) #don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") diff --git a/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py b/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py index a977a6df3..5ceb11079 100644 --- a/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py +++ b/examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py @@ -1,5 +1,7 @@ import pybullet as p import time +import pybullet_data + #p.connect(p.UDP,"192.168.86.100") cid = p.connect(p.SHARED_MEMORY) @@ -7,6 +9,8 @@ if (cid < 0): p.connect(p.GUI) p.resetSimulation() +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + meshScale = [.1, .1, .01] shift = [0, 0, 0] diff --git a/examples/pybullet/examples/vr_kuka_control.py b/examples/pybullet/examples/vr_kuka_control.py index 14e67e276..203c9958c 100644 --- a/examples/pybullet/examples/vr_kuka_control.py +++ b/examples/pybullet/examples/vr_kuka_control.py @@ -3,9 +3,13 @@ import pybullet as p import math # import numpy as np +import pybullet_data + p.connect(p.SHARED_MEMORY) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + kuka = 3 kuka_gripper = 7 POSITION = 1 diff --git a/examples/pybullet/examples/vr_kuka_pr2_move.py b/examples/pybullet/examples/vr_kuka_pr2_move.py index 1af2bd265..c671097ba 100644 --- a/examples/pybullet/examples/vr_kuka_pr2_move.py +++ b/examples/pybullet/examples/vr_kuka_pr2_move.py @@ -1,8 +1,12 @@ #python script with hardcoded values, assumes that you run the vr_kuka_setup.py first import pybullet as p +import pybullet_data + p.connect(p.SHARED_MEMORY) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + pr2_gripper = 2 pr2_cid = 1 diff --git a/examples/pybullet/examples/vr_kuka_setup.py b/examples/pybullet/examples/vr_kuka_setup.py index f6a90bbf7..da9a795c7 100644 --- a/examples/pybullet/examples/vr_kuka_setup.py +++ b/examples/pybullet/examples/vr_kuka_setup.py @@ -1,8 +1,11 @@ import pybullet as p import time #p.connect(p.UDP,"192.168.86.100") +import pybullet_data cid = p.connect(p.SHARED_MEMORY) + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) if (cid < 0): p.connect(p.GUI) p.resetSimulation() diff --git a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py index 17b6637d8..2d3ebaf0b 100644 --- a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py +++ b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py @@ -1,11 +1,15 @@ import pybullet as p import time #p.connect(p.UDP,"192.168.86.100") +import pybullet_data + cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) p.resetSimulation() + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) objects = [ diff --git a/examples/pybullet/examples/vr_kuka_setup_vrSyncPython.py b/examples/pybullet/examples/vr_kuka_setup_vrSyncPython.py index 989df74d2..ef9ac0dae 100644 --- a/examples/pybullet/examples/vr_kuka_setup_vrSyncPython.py +++ b/examples/pybullet/examples/vr_kuka_setup_vrSyncPython.py @@ -1,11 +1,14 @@ import pybullet as p import time #p.connect(p.UDP,"192.168.86.100") +import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) p.resetSimulation() + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #disable rendering during loading makes it much faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) objects = [ diff --git a/examples/pybullet/examples/vr_racecar_differential.py b/examples/pybullet/examples/vr_racecar_differential.py index c664967f0..6a8a64545 100644 --- a/examples/pybullet/examples/vr_racecar_differential.py +++ b/examples/pybullet/examples/vr_racecar_differential.py @@ -4,11 +4,13 @@ CONTROLLER_ID = 0 POSITION = 1 ORIENTATION = 2 BUTTONS = 6 +import pybullet_data cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() p.setGravity(0, 0, -10) useRealTimeSim = 1 diff --git a/examples/pybullet/examples/vrhand.py b/examples/pybullet/examples/vrhand.py index 298957e71..3fc0981f3 100644 --- a/examples/pybullet/examples/vrhand.py +++ b/examples/pybullet/examples/vrhand.py @@ -8,12 +8,15 @@ import serial import time import pybullet as p +import pybullet_data #first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) if (c < 0): c = p.connect(p.GUI) #p.resetSimulation() + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) print(c) if (c < 0): diff --git a/examples/pybullet/examples/vrhand_vive_tracker.py b/examples/pybullet/examples/vrhand_vive_tracker.py index 9fc3c3988..de382d7f9 100644 --- a/examples/pybullet/examples/vrhand_vive_tracker.py +++ b/examples/pybullet/examples/vrhand_vive_tracker.py @@ -8,12 +8,14 @@ import serial import time import pybullet as p +import pybullet_data #first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) if (c < 0): c = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setInternalSimFlags(0) #don't load default robot assets etc p.resetSimulation() diff --git a/examples/pybullet/examples/vrminitaur.py b/examples/pybullet/examples/vrminitaur.py index 4039c80d5..7966900e8 100644 --- a/examples/pybullet/examples/vrminitaur.py +++ b/examples/pybullet/examples/vrminitaur.py @@ -2,12 +2,16 @@ import time import pybullet as p +import pybullet_data + #first try to connect to shared memory (VR), if it fails use local GUI c = p.connect(p.SHARED_MEMORY) if (c < 0): c = p.connect(p.GUI) p.resetSimulation() + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, 0) print(c) if (c < 0): diff --git a/examples/pybullet/examples/vrtracker.py b/examples/pybullet/examples/vrtracker.py index 500e36947..745caffeb 100644 --- a/examples/pybullet/examples/vrtracker.py +++ b/examples/pybullet/examples/vrtracker.py @@ -4,6 +4,8 @@ # Line width can be changed import pybullet as p +import pybullet_data + CONTROLLER_ID = 0 POSITION = 1 @@ -16,6 +18,7 @@ print(c) if (c < 0): p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setInternalSimFlags(0) #don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") diff --git a/examples/pybullet/examples/widows.py b/examples/pybullet/examples/widows.py index 9ecc5d256..226c7f6b5 100644 --- a/examples/pybullet/examples/widows.py +++ b/examples/pybullet/examples/widows.py @@ -1,7 +1,9 @@ import pybullet as p import time +import pybullet_data p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("table/table.urdf", 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) p.setGravity(0, 0, -10) arm = p.loadURDF("widowx/widowx.urdf", useFixedBase=True) diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index 8f67de5d6..b6713bfd6 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -181,12 +181,12 @@ register( reward_threshold=18.0, ) -register( - id='StrikerBulletEnv-v0', - entry_point='pybullet_envs.gym_manipulator_envs:StrikerBulletEnv', - max_episode_steps=100, - reward_threshold=18.0, -) +#register( +# id='StrikerBulletEnv-v0', +# entry_point='pybullet_envs.gym_manipulator_envs:StrikerBulletEnv', +# max_episode_steps=100, +# reward_threshold=18.0, +#) register(id='Walker2DBulletEnv-v0', entry_point='pybullet_envs.gym_locomotion_envs:Walker2DBulletEnv', diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index f9c51c051..ac7c2c067 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -26,7 +26,7 @@ class MJCFBaseBulletEnv(gym.Env): self.scene = None self.physicsClientId = -1 self.ownsPhysicsClient = 0 - self.camera = Camera() + self.camera = Camera(self) self.isRender = render self.robot = robot self.seed() @@ -88,16 +88,23 @@ class MJCFBaseBulletEnv(gym.Env): self.potential = self.robot.calc_potential() return s + def camera_adjust(self): + pass + def render(self, mode='human', close=False): + if mode == "human": self.isRender = True + if self.physicsClientId>=0: + self.camera_adjust() + if mode != "rgb_array": return np.array([]) base_pos = [0, 0, 0] if (hasattr(self, 'robot')): - if (hasattr(self.robot, 'body_xyz')): - base_pos = self.robot.body_xyz + if (hasattr(self.robot, 'body_real_xyz')): + base_pos = self.robot.body_real_xyz if (self.physicsClientId>=0): view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -115,15 +122,8 @@ class MJCFBaseBulletEnv(gym.Env): viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) - try: - # Keep the previous orientation of the camera set by the user. - con_mode = self._p.getConnectionInfo()['connectionMethod'] - if con_mode==self._p.SHARED_MEMORY or con_mode == self._p.GUI: - [yaw, pitch, dist] = self._p.getDebugVisualizerCamera()[8:11] - self._p.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos) - except: - pass + self._p.configureDebugVisualizer(self._p.COV_ENABLE_SINGLE_STEP_RENDERING,1) else: px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8) rgb_array = np.array(px, dtype=np.uint8) @@ -160,11 +160,15 @@ class MJCFBaseBulletEnv(gym.Env): class Camera: - def __init__(self): + def __init__(self, env): + self.env = env pass def move_and_look_at(self, i, j, k, x, y, z): lookat = [x, y, z] - distance = 10 - yaw = 10 - self._p.resetDebugVisualizerCamera(distance, yaw, -20, lookat) + camInfo = self.env._p.getDebugVisualizerCamera() + + distance = camInfo[10] + pitch = camInfo[9] + yaw = camInfo[8] + self.env._p.resetDebugVisualizerCamera(distance, yaw, pitch, lookat) diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py index 9fc5d7962..29f5db65d 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py @@ -4,7 +4,6 @@ import inspect currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0, parentdir) -import pybullet import gym import numpy as np import pybullet_envs @@ -32,8 +31,8 @@ class SmallReactivePolicy: def main(): - pybullet.connect(pybullet.DIRECT) env = gym.make("AntBulletEnv-v0") + env.render(mode="human") pi = SmallReactivePolicy(env.observation_space, env.action_space) @@ -55,7 +54,6 @@ def main(): frame += 1 distance = 5 yaw = 0 - still_open = env.render("human") if still_open == False: return diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py index 50eacc552..b95ff2e31 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py @@ -49,7 +49,6 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - still_open = env.render("human") if still_open == False: return diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py index a58b79411..f341aaf00 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HopperBulletEnv_v0_2017may.py @@ -51,7 +51,6 @@ def main(): obs, r, done, _ = env.step(a) score += r frame += 1 - still_open = env.render("human") if still_open == False: return diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py index 572d495ca..18b902d5a 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py @@ -43,16 +43,15 @@ def main(): obs = env.reset() while 1: + time.sleep(1. / 60.) a = pi.act(obs) obs, r, done, _ = env.step(a) score += r frame += 1 - time.sleep(1. / 60.) - still_open = env.render("human") if still_open == False: return - if not done: continue + continue if restart_delay == 0: print("score=%0.2f in %i frames" % (score, frame)) restart_delay = 60 * 2 # 2 sec at 60 fps diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py index 84220d533..cfbbbfd73 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py @@ -45,12 +45,13 @@ def demo_run(): obs = env.reset() while 1: + if (gui): + time.sleep(1. / 60) + a = pi.act(obs) obs, r, done, _ = env.step(a) score += r frame += 1 - if (gui): - time.sleep(1. / 60) still_open = env.render("human") diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index cbf096600..746b79781 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -23,6 +23,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): frame_skip=4) return self.stadium_scene + def reset(self): if (self.stateId >= 0): #print("restoreState self.stateId:",self.stateId) @@ -125,9 +126,10 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): return state, sum(self.rewards), bool(done), {} def camera_adjust(self): - x, y, z = self.body_xyz - self.camera_x = 0.98 * self.camera_x + (1 - 0.98) * x - self.camera.move_and_look_at(self.camera_x, y - 2.0, 1.4, x, y, 1.0) + x, y, z = self.robot.body_real_xyz + + self.camera_x = x + self.camera.move_and_look_at(self.camera_x, y , 1.4, x, y, 1.0) class HopperBulletEnv(WalkerBaseBulletEnv): diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 43ff604ac..993aaa42c 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -44,6 +44,7 @@ class WalkerBase(MJCFBasedRobot): parts_xyz = np.array([p.pose().xyz() for p in self.parts.values()]).flatten() self.body_xyz = (parts_xyz[0::3].mean(), parts_xyz[1::3].mean(), body_pose.xyz()[2] ) # torso z is more informative than mean z + self.body_real_xyz = body_pose.xyz() self.body_rpy = body_pose.rpy() z = self.body_xyz[2] if self.initial_z == None: diff --git a/setup.py b/setup.py index 4f789d411..f10845c0b 100644 --- a/setup.py +++ b/setup.py @@ -375,7 +375,6 @@ egl_renderer_sources = \ +["src/LinearMath/btConvexHull.cpp"]\ +["src/LinearMath/btConvexHullComputer.cpp"] \ +["src/LinearMath/btGeometryUtil.cpp"]\ -+["src/LinearMath/btReducedVector.cpp"]\ +["src/LinearMath/btQuickprof.cpp"] \ +["src/LinearMath/btThreads.cpp"] \ +["src/Bullet3Common/b3AlignedAllocator.cpp"] \ @@ -502,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.7.3', + version='2.7.4', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index f4a2d5e36..56011899c 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -61,7 +61,8 @@ public: virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0; virtual int getNumOverlappingPairs() const = 0; - + virtual bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const = 0; + virtual btOverlapFilterCallback* getOverlapFilterCallback() = 0; virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0; virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; @@ -380,6 +381,14 @@ public: { } + bool needsBroadphaseCollision(btBroadphaseProxy*, btBroadphaseProxy*) const + { + return true; + } + btOverlapFilterCallback* getOverlapFilterCallback() + { + return 0; + } virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) { } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index a3c9f42eb..fb15ae31e 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -800,6 +800,14 @@ public: ///don't do CCD when the collision filters are not matching if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; + if (m_pairCache->getOverlapFilterCallback()) { + btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle(); + bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1); + if (!collides) + { + return false; + } + } btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index f1b50b39c..27fdead76 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -384,6 +384,9 @@ void btRigidBody::integrateVelocities(btScalar step) { m_angularVelocity *= (MAX_ANGVEL / step) / angvel; } + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } btQuaternion btRigidBody::getOrientation() const diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 39d47cbbd..943d724cc 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -305,6 +305,9 @@ public: void applyTorque(const btVector3& torque) { m_totalTorque += torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_totalTorque); + #endif } void applyForce(const btVector3& force, const btVector3& rel_pos) @@ -316,11 +319,17 @@ public: void applyCentralImpulse(const btVector3& impulse) { m_linearVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif } void applyTorqueImpulse(const btVector3& torque) { m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) @@ -361,20 +370,46 @@ public: { m_pushVelocity = v; } - + + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + void clampVelocity(btVector3& v) const { + v.setX( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getX())) + ); + v.setY( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getY())) + ); + v.setZ( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) + ); + } + #endif + void setTurnVelocity(const btVector3& v) { m_turnVelocity = v; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_turnVelocity); + #endif } void applyCentralPushImpulse(const btVector3& impulse) { m_pushVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_pushVelocity); + #endif } void applyTorqueTurnImpulse(const btVector3& torque) { m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_turnVelocity); + #endif } void clearForces() @@ -408,12 +443,18 @@ public: { m_updateRevision++; m_linearVelocity = lin_vel; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif } inline void setAngularVelocity(const btVector3& ang_vel) { m_updateRevision++; m_angularVelocity = ang_vel; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 4e89e2bce..e70e94f5f 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -29,6 +29,10 @@ subject to the following restrictions: static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray& leafNodes, btAlignedObjectArray >& adj) { int N = leafNodes.size(); + if (N == 0) + { + return NULL; + } while (N > 1) { btAlignedObjectArray marked;