From 0c3a3cc4667eab6ac9fe485bbd7d600cf3277d70 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 08:37:42 -0700 Subject: [PATCH 1/7] pybullet.changeDynamicsInfo/b3ChangeDynamicsInfoSetContactStiffnessAndDamping expose contactStiffness/contactDamping --- examples/SharedMemory/PhysicsClientC_API.cpp | 15 +++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 13 +++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 4 ++++ examples/pybullet/pybullet.c | 12 ++++++++---- 5 files changed, 41 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2ec671d6d..ed1833f2e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1496,6 +1496,8 @@ b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physC struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type = CMD_CHANGE_DYNAMICS_INFO; + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = -1; + command->m_changeDynamicsInfoArgs.m_linkIndex = -2; command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; @@ -1580,6 +1582,19 @@ int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHan return 0; } +int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping) +{ + 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 = linkIndex; + command->m_changeDynamicsInfoArgs.m_contactStiffness =contactStiffness; + command->m_changeDynamicsInfoArgs.m_contactDamping = contactDamping; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING; + return 0; +} + + b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 441395228..4fe58b9fb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -89,6 +89,7 @@ int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHa int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution); int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping); int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); +int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 40385d85c..79057432c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4345,6 +4345,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) { mb->getBaseCollider()->setFriction(lateralFriction); @@ -4392,6 +4396,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } } @@ -4422,6 +4431,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm body->m_rigidBody->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); + } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) { body->m_rigidBody->setRestitution(restitution); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 111dbe1bb..2dfba579e 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -119,6 +119,8 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32, CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING=64, CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128, + CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256, + }; struct ChangeDynamicsInfoArgs @@ -133,6 +135,8 @@ struct ChangeDynamicsInfoArgs double m_restitution; double m_linearDamping; double m_angularDamping; + double m_contactStiffness; + double m_contactDamping; }; struct GetDynamicsInfoArgs diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 770c301de..d394269ac 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -625,12 +625,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double restitution = -1; double linearDamping = -1; double angularDamping = -1; - + double contactStiffness = -1; + double contactDamping = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &physicsClientId)) { return NULL; } @@ -671,12 +672,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetAngularDamping(command,bodyUniqueId,angularDamping); } - if (restitution>=0) { b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution); } + if (contactStiffness>=0 && contactDamping >=0) + { + b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command,bodyUniqueId,linkIndex,contactStiffness, contactDamping); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } From d08f3e5f9187e8bfea7e385bbb2004720d4b9f0c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 09:37:28 -0700 Subject: [PATCH 2/7] expose pybullet non-contact erp, friction erp and frictionAnchor, b3PhysicsParamSetDefaultNonContactERP / b3PhysicsParamSetDefaultFrictionERP / b3ChangeDynamicsInfoSetFrictionAnchor --- examples/SharedMemory/PhysicsClientC_API.cpp | 30 +++++++++++- examples/SharedMemory/PhysicsClientC_API.h | 5 +- .../PhysicsServerCommandProcessor.cpp | 48 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 8 +++- examples/pybullet/pybullet.c | 31 ++++++++++-- .../pybullet/tensorflow/humanoid_running.py | 2 +- 6 files changed, 112 insertions(+), 12 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ed1833f2e..2e335a8ce 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -452,10 +452,24 @@ int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP; return 0; } +int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP; + command->m_physSimParamArgs.m_defaultNonContactERP = defaultNonContactERP; + return 0; +} +int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP; + command->m_physSimParamArgs.m_frictionERP = frictionERP; + return 0; +} -int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); - b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1594,6 +1608,18 @@ int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandl return 0; } +int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor) +{ + 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 = linkIndex; + command->m_changeDynamicsInfoArgs.m_frictionAnchor = frictionAnchor; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR; + return 0; +} + + b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4fe58b9fb..5fd7dd61c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -90,7 +90,7 @@ int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping); int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); - +int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -220,6 +220,9 @@ b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle phys int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); +int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP); +int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP); + int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 79057432c..c89f4af21 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4360,7 +4360,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) { mb->getBaseCollider()->setRollingFriction(rollingFriction); - } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { @@ -4392,6 +4403,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) { mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); @@ -4452,6 +4475,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm body->m_rigidBody->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); + } else + { + body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { btVector3 localInertia; @@ -4568,6 +4602,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) { m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2dfba579e..0ef7a2b68 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -120,6 +120,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING=64, CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128, CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256, + CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512, }; @@ -137,6 +138,7 @@ struct ChangeDynamicsInfoArgs double m_angularDamping; double m_contactStiffness; double m_contactDamping; + int m_frictionAnchor; }; struct GetDynamicsInfoArgs @@ -371,8 +373,8 @@ enum EnumSimParamUpdateFlags SIM_PARAM_MAX_CMD_PER_1MS = 2048, SIM_PARAM_ENABLE_FILE_CACHING = 4096, SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, - - + SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384, + SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, }; enum EnumLoadBunnyUpdateFlags @@ -406,6 +408,8 @@ struct SendPhysicsSimulationParameters int m_collisionFilterMode; int m_enableFileCaching; double m_restitutionVelocityThreshold; + double m_defaultNonContactERP; + double m_frictionERP; }; struct LoadBunnyArgs diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d394269ac..f4b14b62c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -627,11 +627,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double angularDamping = -1; double contactStiffness = -1; double contactDamping = -1; + int frictionAnchor = -1; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddii", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &physicsClientId)) { return NULL; } @@ -681,6 +683,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command,bodyUniqueId,linkIndex,contactStiffness, contactDamping); } + if (frictionAnchor>=0) + { + b3ChangeDynamicsInfoSetFrictionAnchor(command,bodyUniqueId,linkIndex, frictionAnchor); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -757,13 +763,16 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int maxNumCmdPer1ms = -2; int enableFileCaching = -1; double restitutionVelocityThreshold=-1; + double erp; + double contactERP = -1; + double frictionERP = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &physicsClientId)) { return NULL; } @@ -823,6 +832,18 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParamSetEnableFileCaching(command, enableFileCaching); } + if (erp>=0) + { + b3PhysicsParamSetDefaultNonContactERP(command,erp); + } + if (contactERP>=0) + { + b3PhysicsParamSetDefaultContactERP(command,contactERP); + } + if (frictionERP >=0) + { + b3PhysicsParamSetDefaultFrictionERP(command,frictionERP); + } //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); diff --git a/examples/pybullet/tensorflow/humanoid_running.py b/examples/pybullet/tensorflow/humanoid_running.py index c4c741f10..b64310429 100644 --- a/examples/pybullet/tensorflow/humanoid_running.py +++ b/examples/pybullet/tensorflow/humanoid_running.py @@ -188,7 +188,7 @@ def demo_run(): distance=5 yaw = 0 humanPos, humanOrn = p.getBasePositionAndOrientation(human) - p.resetDebugVisualizerCamera(distance,yaw,20,humanPos); + p.resetDebugVisualizerCamera(distance,yaw,-20,humanPos); frame += 1 if frame==1000: break From 60e3887456f3a8914451ad90a5b87d965c2df7ad Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 13:44:34 -0700 Subject: [PATCH 3/7] enable btGearConstraint, expose 'changeDynamics' for gearRatio, only works for maximalCoordinates rigid bodies. See examples\pybullet\examples\mimicJointConstraint.py --- examples/SharedMemory/PhysicsClientC_API.cpp | 11 ++++ examples/SharedMemory/PhysicsClientC_API.h | 4 +- .../PhysicsServerCommandProcessor.cpp | 54 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 3 ++ .../pybullet/examples/mimicJointConstraint.py | 16 ++++++ examples/pybullet/pybullet.c | 12 +++-- 7 files changed, 94 insertions(+), 8 deletions(-) create mode 100644 examples/pybullet/examples/mimicJointConstraint.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2e335a8ce..7c02f71c0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1704,7 +1704,18 @@ int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHan return 0; } +int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_RATIO; + command->m_userConstraintArguments.m_gearRatio = gearRatio; + return 0; +} b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 5fd7dd61c..aefdf469e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -102,7 +102,9 @@ b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHa int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); - +int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c89f4af21..85b75acbb 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5877,7 +5877,48 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + } + else + { + InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + + if (parentBody && childBody) + { + if (parentBody->m_rigidBody) + { + if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + { + btRigidBody* childRb = childBody->m_rigidBody; + if (childRb) + { + btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + + //for now we use the same local axis for both objects + btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + + btScalar ratio=1; + btGearConstraint* gear = new btGearConstraint(*parentBody->m_rigidBody,*childRb, axisA,axisB,ratio); + m_data->m_dynamicsWorld->addConstraint(gear,true); + + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = gear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + } + } } + } } if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) @@ -5921,7 +5962,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (userConstraintPtr->m_rbConstraint) { - //todo + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) + { + btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint; + gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + } + } } serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange; @@ -5944,7 +5992,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (userConstraintPtr->m_rbConstraint) { - + m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); + delete userConstraintPtr->m_rbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); } serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0ef7a2b68..d6f675fa9 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -627,7 +627,7 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_REQUEST_INFO=64, - + USER_CONSTRAINT_CHANGE_GEAR_RATIO=128, }; enum EnumBodyChangeFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index e5b288dbb..d6cc19a56 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -184,6 +184,7 @@ enum JointType { ePlanarType = 3, eFixedType = 4, ePoint2PointType = 5, + eGearType=6 }; @@ -227,6 +228,8 @@ struct b3UserConstraint int m_jointType; double m_maxAppliedForce; int m_userConstraintUniqueId; + double m_gearRatio; + }; struct b3BodyInfo diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py new file mode 100644 index 000000000..a92edc130 --- /dev/null +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -0,0 +1,16 @@ +#mimic joint currently only works for 'maximal coordinate' rigid bodies +#one way to use it would be to attach maximal coordinate rigid bodies to multibody links using +#fixed constraints + +import pybullet as p +import time +p.connect(p.GUI) +wheelA = p.loadURDF("wheel.urdf",[0,0,0],useMaximalCoordinates=1) +wheelB = p.loadURDF("wheel.urdf",[0,0,1],useMaximalCoordinates=1) +c = p.createConstraint(wheelA,-1,wheelB,-1,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-0.1) +p.setRealTimeSimulation(1) +while(1): + time.sleep(0.01) +p.removeConstraint(c) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f4b14b62c..0c7ae9f0e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4608,7 +4608,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "physicsClientId", NULL}; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL}; int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -4620,8 +4620,8 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P double jointChildPivot[3]; double jointChildFrameOrn[4]; double maxForce = -1; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &physicsClientId)) + double gearRatio = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId)) { return NULL; } @@ -4647,7 +4647,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce); } - + if (gearRatio!=0) + { + b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); Py_INCREF(Py_None); @@ -6769,6 +6772,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read + PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read From 46f2f3db4ef8e7de42fccc8dfb8018b61a3b55a0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 16:22:02 -0700 Subject: [PATCH 4/7] implement 'mimic' joint constraint or 'gear' constraint for btMultiBody, add example in pybullet/examples/mimicJointConstraint.py --- .../PhysicsServerCommandProcessor.cpp | 43 ++++- .../pybullet/examples/mimicJointConstraint.py | 19 +- setup.py | 1 + src/BulletDynamics/CMakeLists.txt | 2 + .../Featherstone/btMultiBodyConstraint.h | 1 + .../btMultiBodyGearConstraint.cpp | 169 ++++++++++++++++++ .../Featherstone/btMultiBodyGearConstraint.h | 103 +++++++++++ 7 files changed, 329 insertions(+), 9 deletions(-) create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 85b75acbb..9103e4384 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10,6 +10,10 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h" + + + #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -5759,7 +5763,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); - if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) + + + + if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + { + if (childBody && childBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) + { + btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyGear->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyGear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + } + else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) { if (childBody && childBody->m_multiBody) { @@ -5959,9 +5987,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + } + } if (userConstraintPtr->m_rbConstraint) { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; + //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) { if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py index a92edc130..af615e02f 100644 --- a/examples/pybullet/examples/mimicJointConstraint.py +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -1,16 +1,19 @@ -#mimic joint currently only works for 'maximal coordinate' rigid bodies -#one way to use it would be to attach maximal coordinate rigid bodies to multibody links using -#fixed constraints +#a mimic joint can act as a gear between two joints +#you can control the gear ratio in magnitude and sign (>0 reverses direction) import pybullet as p import time p.connect(p.GUI) -wheelA = p.loadURDF("wheel.urdf",[0,0,0],useMaximalCoordinates=1) -wheelB = p.loadURDF("wheel.urdf",[0,0,1],useMaximalCoordinates=1) -c = p.createConstraint(wheelA,-1,wheelB,-1,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) -p.changeConstraint(c,gearRatio=-0.1) +wheelA = p.loadURDF("wheel.urdf",[0,0,0]) +wheelB = p.loadURDF("wheel.urdf",[0,0,1]) +p.setJointMotorControl2(wheelA,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0) +p.setJointMotorControl2(wheelB,0,p.VELOCITY_CONTROL,targetVelocity=1,force=1) + +c = p.createConstraint(wheelA,0,wheelB,0,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-0.1, maxForce=10000) + p.setRealTimeSimulation(1) while(1): time.sleep(0.01) -p.removeConstraint(c) +#p.removeConstraint(c) \ No newline at end of file diff --git a/setup.py b/setup.py index ecd77a484..11f5ed71c 100644 --- a/setup.py +++ b/setup.py @@ -253,6 +253,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletDynamics/Featherstone/btMultiBody.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"]\ ++["src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"]\ diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 4023d721e..f8a6f34ba 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -37,6 +37,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyFixedConstraint.cpp Featherstone/btMultiBodySliderConstraint.cpp Featherstone/btMultiBodyJointMotor.cpp + Featherstone/btMultiBodyGearConstraint.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp MLCPSolvers/btLemkeAlgorithm.cpp @@ -98,6 +99,7 @@ SET(Featherstone_HDRS Featherstone/btMultiBodyFixedConstraint.h Featherstone/btMultiBodySliderConstraint.h Featherstone/btMultiBodyJointMotor.h + Featherstone/btMultiBodyGearConstraint.h ) SET(MLCPSolvers_HDRS diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 7204f2785..27e1ccc05 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -183,6 +183,7 @@ public: virtual void debugDraw(class btIDebugDraw* drawer)=0; + virtual void setGearRatio(btScalar ratio) {} }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp new file mode 100644 index 000000000..2e242b1f7 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -0,0 +1,169 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyGearConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), + m_gearRatio(1) +{ + +} + +void btMultiBodyGearConstraint::finalizeMultiDof() +{ + + allocateJacobiansMultiDof(); + + m_numDofsFinalized = m_jacSizeBoth; +} + +btMultiBodyGearConstraint::~btMultiBodyGearConstraint() +{ +} + + +int btMultiBodyGearConstraint::getIslandIdA() const +{ + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyGearConstraint::getIslandIdB() const +{ + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + +void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + + if (m_maxAppliedImpulse==0.f) + return; + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF); + + // row 0: the lower bound + jacobianA(0)[offsetA] = 1; + jacobianB(0)[offsetB] = m_gearRatio; + + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + btScalar erp = infoGlobal.m_erp; + btScalar kp = 1; + btScalar kd = 1; + int numRows = getNumRows(); + + for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + + //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + //btScalar velocityError = (m_desiredVelocity - currentVelocity); + + btScalar desiredRelativeVelocity = 0; + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); + + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + { + //expect either prismatic or revolute joint type for now + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eRevolute: + { + constraintRow.m_contactNormal1.setZero(); + constraintRow.m_contactNormal2.setZero(); + btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; + + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1=prismaticAxisInWorld; + constraintRow.m_contactNormal2=-prismaticAxisInWorld; + constraintRow.m_relpos1CrossNormal.setZero(); + constraintRow.m_relpos2CrossNormal.setZero(); + break; + } + default: + { + btAssert(0); + } + }; + + } + + } + +} + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h new file mode 100644 index 000000000..947166ae7 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -0,0 +1,103 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H +#define BT_MULTIBODY_GEAR_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyGearConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btScalar m_gearRatio; + +public: + + btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyGearConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } + + virtual void setGearRatio(btScalar gearRatio) + { + m_gearRatio = gearRatio; + } + +}; + +#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H From c1e32e64284e63c3a748f283d6700129a2b32669 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 19:00:44 -0700 Subject: [PATCH 5/7] fix potential memory leaks --- .../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp | 2 ++ examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp | 8 ++++++++ examples/SharedMemory/PhysicsServerExample.cpp | 5 +++++ 3 files changed, 15 insertions(+) diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index c14658ab9..e0301587a 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -64,8 +64,10 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& meshData.m_textureHeight = height; } else { + b3Warning("Unsupported texture image format [%s]\n",relativeFileName); meshData.m_textureWidth = 0; meshData.m_textureHeight = 0; + break; } } else diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index d243c1bc9..86f707a2f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1243,6 +1243,14 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index) const UrdfCollision& col = link->m_collisionArray[v]; btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix); m_data->m_allocatedCollisionShapes.push_back(childShape); + if (childShape->getShapeType()==COMPOUND_SHAPE_PROXYTYPE) + { + btCompoundShape* compound = (btCompoundShape*) childShape; + for (int i=0;igetNumChildShapes();i++) + { + m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i)); + } + } if (childShape) { diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 1180dcfd8..cbf7427bb 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -723,6 +723,11 @@ public: virtual ~MultiThreadedOpenGLGuiHelper() { //delete m_childGuiHelper; + if (m_debugDraw) + { + delete m_debugDraw; + m_debugDraw = 0; + } } void setCriticalSection(b3CriticalSection* cs) From 9d422c9b080b65a82b05602280ebea7d0eb5c928 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 19:02:23 -0700 Subject: [PATCH 6/7] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 11f5ed71c..235396713 100644 --- a/setup.py +++ b/setup.py @@ -419,7 +419,7 @@ else: setup( name = 'pybullet', - version='1.1.2', + version='1.1.3', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine 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 c1f6b7e9b85bdea10dc39324d72cc13f8c09b1fd Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 8 Jun 2017 09:21:32 -0700 Subject: [PATCH 7/7] fix memory leak in case a Wavefront .obj file had multiple shapes (texture was loaded multiple times, while only releasing one) --- examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index e0301587a..7912b3c49 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -36,7 +36,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& //int textureIndex = -1; //try to load some texture - for (int i=0;i0)