AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

Merge pull request #3351 from erwincoumans/master

Browse Source 
PyBullet: expose useMultiBody argument for loadMJCF command, expose sleeping threshold
This commit is contained in:
erwincoumans 2021-04-20 14:42:42 -07:00 committed by GitHub
commit 201e7159d6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 144 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@ -33,6 +33,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
btScalar sleep_threshold = btScalar(0.22360679775);//sqrt(0.05) to be similar to btMultiBody (SLEEP_THRESHOLD)
rbci.m_angularSleepingThreshold = sleep_threshold;
rbci.m_linearSleepingThreshold = sleep_threshold;
btRigidBody* body = new btRigidBody(rbci);
if (m_rigidBody == 0)
{

23
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -260,6 +260,17 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
}
}
B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_MJCF);
if (command->m_type == CMD_LOAD_MJCF)
{
command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY;
command->m_mjcfArguments.m_useMultiBody = useMultiBody;
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@ -3296,6 +3307,18 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHa
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_sleepThreshold = sleepThreshold;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -156,7 +156,8 @@ extern "C"
B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit);
B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce);
B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType);
B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold);
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
@ -415,6 +416,8 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,

18
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -9761,6 +9761,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD)
{
mb->setSleepThreshold(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold);
}
if (linkIndex == -1)
{
if (mb->getBaseCollider())
@ -10045,6 +10050,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
btRigidBody* rb = 0;
if (body && body->m_rigidBody)
{
if (linkIndex == -1)
{
rb = body->m_rigidBody;
@ -10180,6 +10186,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
m_data->m_dynamicsWorld->addCollisionObject(rb, collisionFilterGroup, collisionFilterMask);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD)
{
btScalar threshold2 = btSqrt(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold);
rb->setSleepingThresholds(threshold2,threshold2);
}
}
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@ -14612,7 +14625,12 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
{
m_data->m_pickedBody = body;
m_data->m_savedActivationState = body->getActivationState();
if (m_data->m_savedActivationState==ISLAND_SLEEPING)
{
m_data->m_savedActivationState = ACTIVE_TAG;
}
m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
m_data->m_pickedBody->setDeactivationTime(0);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -174,6 +174,7 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS = 1 << 18,
CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE = 1 << 19,
CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE = 1 << 20,
CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD = 1 << 21,
};
struct ChangeDynamicsInfoArgs
@ -205,6 +206,9 @@ struct ChangeDynamicsInfoArgs
double m_jointLimitForce;
int m_dynamicType;
double m_sleepThreshold;
};
struct GetDynamicsInfoArgs

53
examples/pybullet/gym/pybullet_examples/sleeptest.py Normal file
View File

@ -0,0 +1,53 @@
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
objects=[]
useMaximalCoordinates=False
if 0:
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_SPHERE, radius = 0.1)
batchPositions = []
index = 1
for x in range(2):
for y in range(1):
for z in range(1):
batchPositions.append(arr[index])
index=index+1
bodyUids = p.createMultiBody(baseMass=10,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=-1,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=useMaximalCoordinates)
for b in bodyUids:
objects.append(b)
else:
for i in range(2):
for j in range(1):
for k in range(1):
ob = p.loadURDF("sphere_1cm.urdf", [0.210050 * i, 0.210050 * j, 1 + 0.210050 * k],globalScaling=20,
useMaximalCoordinates=useMaximalCoordinates)
objects.append(ob)
p.changeDynamics(ob, -1, activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING, linearDamping=0, angularDamping=0, sleepThreshold=0.05)
if (i==0):
p.resetBaseVelocity(ob, [0,0,0], [0,0,0.22])
timeid = p.addUserDebugText("t=", [0,0,2])
lvelid = p.addUserDebugText("lvel", [0,0,1.8])
avelid = p.addUserDebugText("avel", [0,0,1.6])
t=0
dt=1./240.
while p.isConnected():
p.stepSimulation()
t+=dt
txtid = p.addUserDebugText("t="+str(t), [0,0,2],replaceItemUniqueId=timeid)
lin, ang = p.getBaseVelocity(ob)
txtid = p.addUserDebugText("lvel="+"{:.4f}".format(lin[0])+","+"{:.4f}".format(lin[1])+","+"{:.4f}".format(lin[2]), [0,0,1.8],replaceItemUniqueId=lvelid)
txtid = p.addUserDebugText("avel="+"{:.4f}".format(ang[0])+","+"{:.4f}".format(ang[1])+","+"{:.4f}".format(ang[2]), [0,0,1.6],replaceItemUniqueId=avelid)
time.sleep(dt)

23
examples/pybullet/pybullet.c
View File

@ -1354,9 +1354,10 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
PyObject* pylist = 0;
int physicsClientId = 0;
int flags = -1;
int useMultiBody = -1;
static char* kwlist[] = {"mjcfFileName", "flags", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &mjcfFileName, &flags, &physicsClientId))
static char* kwlist[] = {"mjcfFileName", "flags", "useMultiBody", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|iii", kwlist, &mjcfFileName, &flags, &useMultiBody, &physicsClientId))
{
return NULL;
}
@ -1372,6 +1373,11 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
{
b3LoadMJCFCommandSetFlags(command, flags);
}
if (useMultiBody>=0)
{
b3LoadMJCFCommandSetUseMultiBody(command, useMultiBody);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_MJCF_LOADING_COMPLETED)
@ -1379,7 +1385,7 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
return NULL;
}
numBodies =
b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
if (numBodies > MAX_SDF_BODIES)
@ -1430,11 +1436,14 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
double jointUpperLimit = -1;
double jointLimitForce = -1;
double sleepThreshold = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "jointLowerLimit","jointUpperLimit", "jointLimitForce", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "jointLowerLimit","jointUpperLimit", "jointLimitForce", "sleepThreshold", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOddddddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &jointLowerLimit , &jointUpperLimit , &jointLimitForce , &sleepThreshold, &physicsClientId))
{
return NULL;
}
@ -1460,6 +1469,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
{
b3ChangeDynamicsInfoSetJointLimitForce(command, bodyUniqueId, linkIndex, jointLimitForce);
}
if (sleepThreshold >=0)
{
b3ChangeDynamicsInfoSetSleepThreshold(command, bodyUniqueId, sleepThreshold);
}
if (jointLowerLimit <= jointUpperLimit)
{

11
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -33,8 +33,8 @@
namespace
{
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2); // in seconds
} // namespace
void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links,
m_canSleep(canSleep),
m_canWakeup(true),
m_sleepTimer(0),
m_sleepEpsilon(INITIAL_SLEEP_EPSILON),
m_sleepTimeout(INITIAL_SLEEP_TIMEOUT),
m_userObjectPointer(0),
m_userIndex2(-1),
m_userIndex(-1),
@ -2104,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
motion += m_realBuf[i] * m_realBuf[i];
}
if (motion < SLEEP_EPSILON)
if (motion < m_sleepEpsilon)
{
m_sleepTimer += timestep;
if (m_sleepTimer > SLEEP_TIMEOUT)
if (m_sleepTimer > m_sleepTimeout)
{
goToSleep();
}

13
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@ -726,6 +726,17 @@ public:
bool isLinkAndAllAncestorsKinematic(const int i) const;
void setSleepThreshold(btScalar sleepThreshold)
{
m_sleepEpsilon = sleepThreshold;
}
void setSleepTimeout(btScalar sleepTimeout)
{
this->m_sleepTimeout = sleepTimeout;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@ -801,6 +812,8 @@ private:
bool m_canSleep;
bool m_canWakeup;
btScalar m_sleepTimer;
btScalar m_sleepEpsilon;
btScalar m_sleepTimeout;
void *m_userObjectPointer;
int m_userIndex2;