From 4da2c076a705c25a911db8991fdc477132203e2c Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 1 May 2017 22:18:54 -0700 Subject: [PATCH 1/4] Add API to reset mass. --- examples/SharedMemory/PhysicsClientC_API.cpp | 22 ++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 6 +++ .../PhysicsServerCommandProcessor.cpp | 33 +++++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 15 +++++++ examples/SharedMemory/SharedMemoryPublic.h | 7 ++++ examples/pybullet/pybullet.c | 40 +++++++++++++++++++ 6 files changed, 123 insertions(+) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 94bbedc36..4d819a6b5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1208,7 +1208,29 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd return cl->getJointInfo(bodyIndex, jointIndex, *info); } +b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_RESET_DYNAMIC_INFO; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle) command; +} +int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) +{ + 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_mass = mass; + command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_MASS; + 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 dcc210760..339ed2a2d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -74,6 +74,12 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); + +///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h +int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info); + +b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient); +int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); 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 625c99d33..9ef821567 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3732,6 +3732,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; + case CMD_RESET_DYNAMIC_INFO: + { + BT_PROFILE("CMD_RESET_DYNAMIC_INFO"); + + if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_MASS) + { + int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex; + double mass = clientCmd.m_resetDynamicInfoArgs.m_mass; + 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->setBaseMass(mass); + } + else + { + mb->getLink(linkIndex).m_mass = mass; + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + + break; + }; case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f85a62d80..f7c2b91a0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -106,6 +106,20 @@ struct BulletDataStreamArgs char m_bodyName[MAX_FILENAME_LENGTH]; }; +enum EnumResetDynamicInfoFlags +{ + RESET_DYNAMIC_INFO_SET_MASS=1, + RESET_DYNAMIC_INFO_SET_COM=2, +}; + +struct ResetDynamicInfoArgs +{ + int m_bodyUniqueId; + int m_linkIndex; + double m_mass; + double m_COM[3]; +}; + struct SetJointFeedbackArgs { int m_bodyUniqueId; @@ -702,6 +716,7 @@ struct SharedMemoryCommand struct MjcfArgs m_mjcfArguments; struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; + struct ResetDynamicInfoArgs m_resetDynamicInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 466f3dcb3..25f5a2816 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -55,6 +55,7 @@ enum EnumSharedMemoryClientCommand CMD_CONFIGURE_OPENGL_VISUALIZER, CMD_REQUEST_KEYBOARD_EVENTS_DATA, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, + CMD_RESET_DYNAMIC_INFO, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -207,6 +208,12 @@ struct b3BodyInfo const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf }; +struct b3DynamicInfo +{ + double m_mass; + double m_localInertialPosition[3]; + double m_localInertialOrientation[4]; +}; // copied from btMultiBodyLink.h enum SensorType { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8485d357b..5211c8a85 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -604,6 +604,43 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key return pylist; } +static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + int linkIndex = -2; + double mass = -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)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle command = b3InitResetDynamicInfo(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (mass >= 0) + { + b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds) { double fixedTimeStep = -1; @@ -5435,6 +5472,9 @@ static PyMethodDef SpamMethods[] = { {"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS, "Reset the state (position, velocity etc) for a joint on a body " "instantaneously, not through physics simulation."}, + + {"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS, + "Reset dynamic information such as mass, COM."}, {"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS, "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." From c7e9a3189862ad68ae8bc79688cfeebf05d45a7e Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 3 May 2017 18:25:25 -0700 Subject: [PATCH 2/4] Add API to get user constraint id. --- examples/SharedMemory/PhysicsClient.h | 2 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 7 ++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../PhysicsClientSharedMemory.cpp | 9 ++++++ .../SharedMemory/PhysicsClientSharedMemory.h | 2 ++ examples/SharedMemory/PhysicsDirect.cpp | 9 +++++- examples/SharedMemory/PhysicsDirect.h | 2 ++ examples/SharedMemory/PhysicsLoopBack.cpp | 5 +++ examples/SharedMemory/PhysicsLoopBack.h | 2 ++ examples/pybullet/examples/constraint.py | 4 ++- examples/pybullet/pybullet.c | 32 ++++++++++++++++++- 11 files changed, 73 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 797aa86ea..2ce391c60 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -37,6 +37,8 @@ public: virtual int getNumUserConstraints() const = 0; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0; + + virtual int getUserConstraintId(int serialIndex) const = 0; virtual void setSharedMemoryKey(int key) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4d819a6b5..abf228607 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1179,6 +1179,13 @@ int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniq return 0; } +/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) +int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + return cl->getUserConstraintId(serialIndex); +} + /// return the body unique id, given the index in range [0 , b3GetNumBodies() ) int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 339ed2a2d..ae534017d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -96,6 +96,8 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info); +/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) +int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex); ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fdd48a1b3..dcac6541b 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -163,6 +163,15 @@ int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, str return 0; } +int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + PhysicsClientSharedMemory::PhysicsClientSharedMemory() { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 10fc7363d..163ea55fb 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -47,6 +47,8 @@ public: virtual int getNumUserConstraints() const; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 92f53e648..ef1d70aba 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -925,7 +925,14 @@ int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserCo return 0; } - +int PhysicsDirect::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} int PhysicsDirect::getBodyUniqueId(int serialIndex) const { diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 8a9905de4..18e08477d 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -66,6 +66,8 @@ public: virtual int getNumUserConstraints() const; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 69236d71f..bf33bea2e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -108,6 +108,11 @@ int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3User return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info); } +int PhysicsLoopBack::getUserConstraintId(int serialIndex) const +{ + return m_data->m_physicsClient->getUserConstraintId(serialIndex); +} + ///todo: move this out of the interface void PhysicsLoopBack::setSharedMemoryKey(int key) { diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 790571170..cd05027f1 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -51,6 +51,8 @@ public: virtual int getNumUserConstraints() const; virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const; + + virtual int getUserConstraintId(int serialIndex) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 040708664..51ac0750a 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -9,6 +9,8 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1]) +print cid +print p.getConstraintUniqueId(0) prev=[0,0,1] a=-math.pi while 1: @@ -21,4 +23,4 @@ while 1: orn = p.getQuaternionFromEuler([a,0,0]) p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50) -p.removeConstraint(cid) \ No newline at end of file +p.removeConstraint(cid) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5211c8a85..2154d7414 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1846,6 +1846,36 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb return NULL; } +static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int serialIndex = -1; + b3PhysicsClientHandle sm = 0; + + static char* kwlist[] = {"serialIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int userConstraintId = -1; + userConstraintId = b3GetUserConstraintId(sm, serialIndex); + +#if PY_MAJOR_VERSION >= 3 + return PyLong_FromLong(userConstraintId); +#else + return PyInt_FromLong(userConstraintId); +#endif + } +} + static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds) { int numConstraints = 0; @@ -5430,7 +5460,7 @@ static PyMethodDef SpamMethods[] = { {"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, "Get the user-created constraint info, given a constraint unique id."}, - {"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS, + {"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS, "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."}, {"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation, From 1841a41f2af98121b263b18d9e1bd8140850cd8c Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 3 May 2017 21:30:42 -0700 Subject: [PATCH 3/4] Add pybullet example for resetting dynamics. --- examples/SharedMemory/PhysicsClientC_API.cpp | 1 + .../pybullet/examples/reset_dynamic_info.py | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) create mode 100644 examples/pybullet/examples/reset_dynamic_info.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index abf228607..33ca652e1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1232,6 +1232,7 @@ int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bod { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO); + b3Assert(mass > 0); command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex; command->m_resetDynamicInfoArgs.m_mass = mass; diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py new file mode 100644 index 000000000..fc0f3cea1 --- /dev/null +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -0,0 +1,18 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) + +p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593]) +p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2]) +p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4]) +p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1) +#p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0) +p.setGravity(0,0,-10) +p.setRealTimeSimulation(0) +t=0 +while 1: + t=t+1 + time.sleep(.01) + p.stepSimulation() From 939d6ead327848771681a35fb2427e2c798325ae Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 3 May 2017 21:47:53 -0700 Subject: [PATCH 4/4] Add API to reset lateral friction coefficient. --- examples/SharedMemory/PhysicsClientC_API.cpp | 11 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 24 +++++++++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/SharedMemory/SharedMemoryPublic.h | 1 - .../pybullet/examples/reset_dynamic_info.py | 2 ++ examples/pybullet/pybullet.c | 12 +++++++--- src/BulletDynamics/Featherstone/btMultiBody.h | 5 ++++ 8 files changed, 54 insertions(+), 4 deletions(-) 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