From 4da2c076a705c25a911db8991fdc477132203e2c Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 1 May 2017 22:18:54 -0700 Subject: [PATCH] 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."