From b072f2fdc84e416d219af5232cd25a61c046a9d9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Mar 2019 22:05:15 -0800 Subject: [PATCH 1/4] bump up PyBullet version to 2.4.7 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 169872e23..2eb03542d 100644 --- a/setup.py +++ b/setup.py @@ -466,7 +466,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.5', + version='2.4.7', 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', From 58c638d5ee910544f8dabc4f68ddce4bca947c78 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 8 Mar 2019 07:43:50 -0800 Subject: [PATCH 2/4] tweak mouse wheel multiplier --- examples/CommonInterfaces/CommonGraphicsAppInterface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 959e51a40..acfaf478c 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -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); From 32e93d9f912a71f857b918aebcd2ce6090cdaab2 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 8 Mar 2019 09:20:32 -0800 Subject: [PATCH 3/4] allow to PyBullet.changeDynamics for all links in maximal coordinate rigid bodies change snake.py to use useMaximalCoordinate = True by default --- .../ImportURDFDemo/MyMultiBodyCreator.cpp | 10 ++- .../PhysicsServerCommandProcessor.cpp | 68 ++++++++++++------- examples/pybullet/examples/snake.py | 9 ++- 3 files changed, 57 insertions(+), 30 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 905a6cc00..e41ef7881 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -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) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f0f8dc6d2..9de30169f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7876,104 +7876,124 @@ 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.); } } } diff --git a/examples/pybullet/examples/snake.py b/examples/pybullet/examples/snake.py index 52d876aeb..af2c6d512 100644 --- a/examples/pybullet/examples/snake.py +++ b/examples/pybullet/examples/snake.py @@ -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 From 4ea907aacfcd93c88b633138be9185b840d31755 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 9 Mar 2019 09:23:16 -0800 Subject: [PATCH 4/4] expose maxJointVelocity through PyBullet.changeDynamics, this Fixes Issue #1890 bump up PyBullet to version 2.4.8 --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 ++- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 8 +++++++- examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/pybullet/pybullet.c | 12 +++++++++--- setup.py | 2 +- 6 files changed, 34 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 12ef3975a..8eaac0a6f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 3499a8c88..2b67887f0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9de30169f..e04686e81 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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 { @@ -7989,6 +7993,8 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) { rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 7c25bb3f1..5be07b05a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 755fcea13..dfd95e524 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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); } diff --git a/setup.py b/setup.py index 2eb03542d..adb6f3526 100644 --- a/setup.py +++ b/setup.py @@ -466,7 +466,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.7', + 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',