diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8db4665f6..b354cc69f 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -359,7 +359,7 @@ void ConvertURDF2BulletInternal( if (cache.m_bulletMultiBody==0) { - bool canSleep = false; + bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0; bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 3c4339739..deca1c3b9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -29,6 +29,7 @@ enum ConvertURDFFlags { CUF_GLOBAL_VELOCITIES_MB=256, CUF_MJCF_COLORS_FROM_FILE=512, CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, + CUF_ENABLE_SLEEPING=2048, }; struct UrdfVisualShapeCache diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index eae4255a6..c108aa77b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -4513,7 +4513,7 @@ B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient) return -1; } -B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path) +B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 712340dc9..ebf83c677 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -575,7 +575,7 @@ B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryComma B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient); -B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path); +B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path); B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 80bbb3c30..1373bd570 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -682,6 +682,7 @@ enum eURDF_Flags URDF_GLOBAL_VELOCITIES_MB =256, MJCF_COLORS_FROM_FILE=512, URDF_ENABLE_CACHED_GRAPHICS_SHAPES=1024, + URDF_ENABLE_SLEEPING=2048, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/pybullet/examples/sleepObjectAutoDeactivation.py b/examples/pybullet/examples/sleepObjectAutoDeactivation.py new file mode 100644 index 000000000..c0a3bfb93 --- /dev/null +++ b/examples/pybullet/examples/sleepObjectAutoDeactivation.py @@ -0,0 +1,25 @@ +import pybullet as p +import time +useMaximalCoordinates=False + +flags = 0#p.URDF_ENABLE_SLEEPING + +p.connect(p.GUI) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) + +p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates) +#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags) +for k in range (10): + for i in range (10): + r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags) + for j in range (p.getNumJoints(r2d2)): + p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0) + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +timestep = 1./240. +p.setTimeStep(timestep) +p.setGravity(0,0,-10) + +while p.isConnected(): + p.stepSimulation() + time.sleep(timestep) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 31b201ea0..e3952e083 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9308,6 +9308,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_GLOBAL_VELOCITIES_MB", URDF_GLOBAL_VELOCITIES_MB); PyModule_AddIntConstant(m, "MJCF_COLORS_FROM_FILE", MJCF_COLORS_FROM_FILE); PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES); + PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); diff --git a/setup.py b/setup.py index d7e69ab68..7a3b2f443 100644 --- a/setup.py +++ b/setup.py @@ -450,7 +450,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.0', + version='2.0.1', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',