diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 33ca652e1..61e3a69d6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1240,6 +1240,17 @@ int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bod return 0; } +int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO); + command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex; + command->m_resetDynamicInfoArgs.m_lateralFriction = lateralFriction; + command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION; + 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 ae534017d..f57ffdcee 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -80,6 +80,7 @@ int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int lin b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient); int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); +int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); 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 9ef821567..873c8670b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3759,6 +3759,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION) + { + int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex; + double lateralFriction = clientCmd.m_resetDynamicInfoArgs.m_lateralFriction; + btAssert(bodyUniqueId >= 0); + btAssert(linkIndex >= -1); + + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + mb->getBaseCollider()->setFriction(lateralFriction); + } + else + { + mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f7c2b91a0..1c141e97d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -110,6 +110,7 @@ enum EnumResetDynamicInfoFlags { RESET_DYNAMIC_INFO_SET_MASS=1, RESET_DYNAMIC_INFO_SET_COM=2, + RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION=4, }; struct ResetDynamicInfoArgs @@ -118,6 +119,7 @@ struct ResetDynamicInfoArgs int m_linkIndex; double m_mass; double m_COM[3]; + double m_lateralFriction; }; struct SetJointFeedbackArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 25f5a2816..e658508a2 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -212,7 +212,6 @@ struct b3DynamicInfo { double m_mass; double m_localInertialPosition[3]; - double m_localInertialOrientation[4]; }; // copied from btMultiBodyLink.h diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py index fc0f3cea1..126312b07 100644 --- a/examples/pybullet/examples/reset_dynamic_info.py +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -14,5 +14,7 @@ p.setRealTimeSimulation(0) t=0 while 1: t=t+1 + if t > 400: + p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) time.sleep(.01) p.stepSimulation() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2154d7414..3fb92f323 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -609,11 +609,12 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj int bodyUniqueId = -1; int linkIndex = -2; double mass = -1; + double lateralFriction = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|di", kwlist, &bodyUniqueId, &linkIndex,&mass, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &physicsClientId)) { return NULL; } @@ -634,6 +635,11 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass); } + if (lateralFriction >= 0) + { + b3ResetDynamicInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -5504,7 +5510,7 @@ static PyMethodDef SpamMethods[] = { "instantaneously, not through physics simulation."}, {"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS, - "Reset dynamic information such as mass, COM."}, + "Reset dynamic information such as mass, lateral friction coefficient."}, {"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS, "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index ea5e90521..8d89cac7d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -140,6 +140,11 @@ public: return m_baseCollider; } + btMultiBodyLinkCollider* getLinkCollider(int index) + { + return m_colliders[index]; + } + // // get parent // input: link num from 0 to num_links-1