From 0c3a3cc4667eab6ac9fe485bbd7d600cf3277d70 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 08:37:42 -0700 Subject: [PATCH] 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); }