From 8567f6bb7caf47d5a723969cafe6f60e99ad386c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 30 May 2018 21:14:35 -0700 Subject: [PATCH] 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)