Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #2153 from erwincoumans/master

Browse Source 
expose maxJointVelocity through PyBullet.changeDynamics, fix mouse wheel multiplier
This commit is contained in:
erwincoumans
2019-03-09 09:25:05 -08:00
committed by GitHub
parent 2b5eabef18 4ea907aacf
commit 31ce4e0e57
9 changed files with 92 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CommonInterfaces/CommonGraphicsAppInterface.h
View File

@@ -251,7 +251,7 @@ struct CommonGraphicsApp
float cameraDistance = camera->getCameraDistance();
if (deltay < 0 || cameraDistance > 1)
{
cameraDistance -= deltay*m_wheelMultiplier * 0.01f;
cameraDistance -= deltay*m_wheelMultiplier;
if (cameraDistance < 1)
cameraDistance = 1;
camera->setCameraDistance(cameraDistance);

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

@@ -33,9 +33,13 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
m_rigidBody = new btRigidBody(rbci);
return m_rigidBody;
btRigidBody* body = new btRigidBody(rbci);
if (m_rigidBody == 0)
{
//only store the root of the multi body
m_rigidBody = body;
}
return body;
}
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2928,6 +2928,19 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
command->m_changeDynamicsInfoArgs.m_maxJointVelocity = maxJointVelocity;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -164,7 +164,8 @@ extern "C"
B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double ccdSweptSphereRadius);
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState);
B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand2(b3SharedMemoryCommandHandle commandHandle, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);

76
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7804,7 +7804,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY)
{
mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
}
}
else
{
@@ -7876,104 +7880,126 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
else
{
btRigidBody* rb = 0;
if (body && body->m_rigidBody)
{
if (linkIndex == -1)
{
rb = body->m_rigidBody;
}
else
{
if (linkIndex >= 0 && linkIndex < body->m_rigidBodyJoints.size())
{
btRigidBody* parentRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyA();
btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
rb = childRb;
}
}
}
if (rb)
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
{
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
{
body->m_rigidBody->forceActivationState(ACTIVE_TAG);
rb->forceActivationState(ACTIVE_TAG);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
{
body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
rb->forceActivationState(DISABLE_DEACTIVATION);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
{
body->m_rigidBody->forceActivationState(ACTIVE_TAG);
rb->forceActivationState(ACTIVE_TAG);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
{
body->m_rigidBody->forceActivationState(ISLAND_SLEEPING);
rb->forceActivationState(ISLAND_SLEEPING);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
btScalar angDamping = body->m_rigidBody->getAngularDamping();
body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
btScalar angDamping = rb->getAngularDamping();
rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
btScalar linDamping = body->m_rigidBody->getLinearDamping();
body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
btScalar linDamping = rb->getLinearDamping();
rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{
body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
rb->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
body->m_rigidBody->setRestitution(restitution);
rb->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
body->m_rigidBody->setFriction(lateralFriction);
rb->setFriction(lateralFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{
body->m_rigidBody->setSpinningFriction(spinningFriction);
rb->setSpinningFriction(spinningFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
body->m_rigidBody->setRollingFriction(rollingFriction);
rb->setRollingFriction(rollingFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
{
if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
{
body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
}
else
{
body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
rb->setCollisionFlags(rb->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
btVector3 localInertia;
if (body->m_rigidBody->getCollisionShape())
if (rb->getCollisionShape())
{
body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass, localInertia);
rb->getCollisionShape()->calculateLocalInertia(mass, localInertia);
}
body->m_rigidBody->setMassProps(mass, localInertia);
rb->setMassProps(mass, localInertia);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
btScalar orgMass = body->m_rigidBody->getInvMass();
btScalar orgMass = rb->getInvMass();
if (orgMass > 0)
{
body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal);
rb->setMassProps(mass, newLocalInertiaDiagonal);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
{
body->m_rigidBody->setAnisotropicFriction(anisotropicFriction);
rb->setAnisotropicFriction(anisotropicFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
{
body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{
body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
//for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
}
}
}

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -165,6 +165,7 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192,
CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384,
CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768,
CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY = 1<<16,
};
struct ChangeDynamicsInfoArgs
@@ -188,6 +189,7 @@ struct ChangeDynamicsInfoArgs
int m_activationState;
double m_jointDamping;
double m_anisotropicFriction[3];
double m_maxJointVelocity;
};
struct GetDynamicsInfoArgs

9
examples/pybullet/examples/snake.py
View File

@@ -12,9 +12,10 @@ plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,plane)
useMaximalCoordinates = False
useMaximalCoordinates = True
sphereRadius = 0.25
colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
@@ -49,10 +50,12 @@ sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrien
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
anistropicFriction = [1,0.01,0.01]
p.changeDynamics(sphereUid,-1,lateralFriction=2,anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid)
for i in range (p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid,i)
p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=[1,0.01,0.01])#0,0,0])#1,0.01,0.01])
p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=anistropicFriction)
dt = 1./240.
SNAKE_NORMAL_PERIOD=0.1#1.5

12
examples/pybullet/pybullet.c
View File

@@ -1243,12 +1243,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
double jointDamping = -1;
PyObject* localInertiaDiagonalObj = 0;
PyObject* anisotropicFrictionObj = 0;
double maxJointVelocity = -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", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId))
{
return NULL;
}
@@ -1336,6 +1337,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
{
b3ChangeDynamicsInfoSetContactProcessingThreshold(command, bodyUniqueId, linkIndex, contactProcessingThreshold);
}
if (maxJointVelocity >= 0)
{
b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}

2
setup.py
View File

@@ -466,7 +466,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name = 'pybullet',
version='2.4.5',
version='2.4.8',
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',