From d08f3e5f9187e8bfea7e385bbb2004720d4b9f0c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 09:37:28 -0700 Subject: [PATCH] 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