From 491ecf2a2063188eaaf02c005ac9cebf21219a9f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 30 May 2018 18:37:08 -0700 Subject: [PATCH] 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);