PyBullet: rename sleepObjectAutoDeactivation -> sleeping.py
PyBullet: allow maximal coordinate rigid bodies to sleep/deactivate
This commit is contained in:
@@ -33,7 +33,7 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
|
|||||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
||||||
rbci.m_startWorldTransform = initialWorldTrans;
|
rbci.m_startWorldTransform = initialWorldTrans;
|
||||||
m_rigidBody = new btRigidBody(rbci);
|
m_rigidBody = new btRigidBody(rbci);
|
||||||
m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
|
|
||||||
|
|
||||||
return m_rigidBody;
|
return m_rigidBody;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -337,6 +337,12 @@ void ConvertURDF2BulletInternal(
|
|||||||
if (!createMultiBody)
|
if (!createMultiBody)
|
||||||
{
|
{
|
||||||
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||||
|
bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
|
||||||
|
if (!canSleep)
|
||||||
|
{
|
||||||
|
body->forceActivationState(DISABLE_DEACTIVATION);
|
||||||
|
}
|
||||||
|
|
||||||
linkRigidBody = body;
|
linkRigidBody = body;
|
||||||
|
|
||||||
world1->addRigidBody(body);
|
world1->addRigidBody(body);
|
||||||
|
|||||||
@@ -2,15 +2,15 @@ import pybullet as p
|
|||||||
import time
|
import time
|
||||||
useMaximalCoordinates=False
|
useMaximalCoordinates=False
|
||||||
|
|
||||||
flags = 0#p.URDF_ENABLE_SLEEPING
|
flags = p.URDF_ENABLE_SLEEPING
|
||||||
|
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||||
|
|
||||||
p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates)
|
p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates)
|
||||||
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
|
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
|
||||||
for k in range (10):
|
for k in range (5):
|
||||||
for i in range (10):
|
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)
|
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)):
|
for j in range (p.getNumJoints(r2d2)):
|
||||||
p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0)
|
p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0)
|
||||||
Reference in New Issue
Block a user