From 2819ff4a00172316c53734c3b772bc0b7fdc5c8f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 29 May 2018 21:05:12 -0700 Subject: [PATCH 1/5] bump up version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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', From 1dec33d44bb347576dec0482a153ed5efafcda71 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 30 May 2018 12:31:29 -0700 Subject: [PATCH 2/5] move default linearslop to 0 --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index fa8f8da40..e74533c34 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2301,7 +2301,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP - m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0; m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7; // m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2; From 14c6dee6a34ba9be096d172de7b40d4ce723c8f6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 30 May 2018 14:37:49 -0700 Subject: [PATCH 3/5] const char* for path --- examples/SharedMemory/PhysicsClientC_API.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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*/]); From 491ecf2a2063188eaaf02c005ac9cebf21219a9f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 30 May 2018 18:37:08 -0700 Subject: [PATCH 4/5] export object auto-deactivation (sleeping) to pybullet (needs some more testing/configuring) --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 1 + .../examples/sleepObjectAutoDeactivation.py | 25 +++++++++++++++++++ examples/pybullet/pybullet.c | 1 + 5 files changed, 29 insertions(+), 1 deletion(-) create mode 100644 examples/pybullet/examples/sleepObjectAutoDeactivation.py 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/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); From 8567f6bb7caf47d5a723969cafe6f60e99ad386c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 30 May 2018 21:14:35 -0700 Subject: [PATCH 5/5] PyBullet: rename sleepObjectAutoDeactivation -> sleeping.py PyBullet: allow maximal coordinate rigid bodies to sleep/deactivate --- examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp | 2 +- examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 6 ++++++ .../{sleepObjectAutoDeactivation.py => sleeping.py} | 6 +++--- 3 files changed, 10 insertions(+), 4 deletions(-) rename examples/pybullet/examples/{sleepObjectAutoDeactivation.py => sleeping.py} (90%) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 3d5b32cd6..6ae5f4b7c 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -33,7 +33,7 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); rbci.m_startWorldTransform = initialWorldTrans; m_rigidBody = new btRigidBody(rbci); - m_rigidBody->forceActivationState(DISABLE_DEACTIVATION); + return m_rigidBody; } diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index b354cc69f..00f1f77ab 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -337,6 +337,12 @@ void ConvertURDF2BulletInternal( if (!createMultiBody) { btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape); + bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0; + if (!canSleep) + { + body->forceActivationState(DISABLE_DEACTIVATION); + } + linkRigidBody = body; world1->addRigidBody(body); diff --git a/examples/pybullet/examples/sleepObjectAutoDeactivation.py b/examples/pybullet/examples/sleeping.py similarity index 90% rename from examples/pybullet/examples/sleepObjectAutoDeactivation.py rename to examples/pybullet/examples/sleeping.py index c0a3bfb93..dbd0d0ec3 100644 --- a/examples/pybullet/examples/sleepObjectAutoDeactivation.py +++ b/examples/pybullet/examples/sleeping.py @@ -2,15 +2,15 @@ import pybullet as p import time useMaximalCoordinates=False -flags = 0#p.URDF_ENABLE_SLEEPING +flags = 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): +for k in range (5): + for i in range (5): 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)