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); 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/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 f0f8dc6d2..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 { @@ -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.); } } } 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/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 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 169872e23..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.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',