From e363e12ea4e2cc6a47a41a8b8a11abe69288b7c2 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 7 May 2017 21:09:08 -0700 Subject: [PATCH 1/5] Add default specular when there is not specular map. Add example for adjusting specular coefficient. --- examples/SharedMemory/PhysicsClientC_API.cpp | 6 +++- .../SharedMemory/PhysicsClientExample.cpp | 35 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/TinyRenderer/model.cpp | 8 +++-- 5 files changed, 47 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7fb2fb30a..be95bab46 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1208,13 +1208,17 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) return cl->getNumJoints(bodyId); } - int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getJointInfo(bodyIndex, jointIndex, *info); } +int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info) +{ + return 0; +} + b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index d0090f6b5..b744a2d0b 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -53,6 +53,9 @@ protected: int m_canvasRGBIndex; int m_canvasDepthIndex; int m_canvasSegMaskIndex; + + float m_lightPos[3]; + float m_specularCoeff; void createButton(const char* name, int id, bool isTrigger ); @@ -257,7 +260,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { #ifdef BT_DEBUG - b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf"); #else b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf");//two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf"); #endif @@ -278,6 +281,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); + b3RequestCameraImageSetLightDirection(commandHandle, m_lightPos); + b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -522,6 +527,7 @@ m_canvas(0), m_canvasRGBIndex(-1), m_canvasDepthIndex(-1), m_canvasSegMaskIndex(-1), +m_specularCoeff(1.0), m_numMotors(0), m_options(options), m_isOptionalServerConnected(false) @@ -660,6 +666,29 @@ void PhysicsClientExample::createButtons() } } } + + { + SliderParams sliderLightPosX("light source position x",&m_lightPos[0]); + SliderParams sliderLightPosY("light source position y",&m_lightPos[1]); + SliderParams sliderLightPosZ("light source position z",&m_lightPos[2]); + SliderParams sliderSpecularCoeff("specular coefficient",&m_specularCoeff); + sliderLightPosX.m_minVal=-1.5; + sliderLightPosX.m_maxVal=1.5; + sliderLightPosY.m_minVal=-1.5; + sliderLightPosY.m_maxVal=1.5; + sliderLightPosZ.m_minVal=-1.5; + sliderLightPosZ.m_maxVal=1.5; + sliderSpecularCoeff.m_minVal=0; + sliderSpecularCoeff.m_maxVal=5.0; + if (m_guiHelper && m_guiHelper->getParameterInterface()) + { + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosX); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosY); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosZ); + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderSpecularCoeff); + } + + } } } @@ -684,6 +713,10 @@ void PhysicsClientExample::initPhysics() m_selectedBody = -1; m_prevSelectedBody = -1; + + m_lightPos[0] = 1.0; + m_lightPos[1] = 1.0; + m_lightPos[2] = 1.0; { m_canvas = m_guiHelper->get2dCanvasInterface(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c0a7a3315..5e339430a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -832,6 +832,7 @@ struct SharedMemoryStatus struct StateLoggingResultArgs m_stateLoggingResultArgs; struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs; struct b3ObjectArgs m_removeObjectArgs; + struct b3DynamicInfo m_dynamicInfo; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 410ca5d3e..7602e309e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -224,6 +224,7 @@ struct b3DynamicInfo { double m_mass; double m_localInertialPosition[3]; + double m_lateralFrictionCoeff; }; // copied from btMultiBodyLink.h diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 056ed001f..25c7701df 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -167,8 +167,12 @@ Vec2f Model::uv(int iface, int nthvert) { } float Model::specular(Vec2f uvf) { - Vec2i uv(uvf[0]*specularmap_.get_width(), uvf[1]*specularmap_.get_height()); - return specularmap_.get(uv[0], uv[1])[0]/1.f; + if (specularmap_.get_width() && specularmap_.get_height()) + { + Vec2i uv(uvf[0]*specularmap_.get_width(), uvf[1]*specularmap_.get_height()); + return specularmap_.get(uv[0], uv[1])[0]/1.f; + } + return 2.0; } Vec3f Model::normal(int iface, int nthvert) { From 5fe4c6bb5b87e14359a303de8638b0e257bb5dbe Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 7 May 2017 22:21:38 -0700 Subject: [PATCH 2/5] Add API to get dynamic info. --- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +++++++++++++-- examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../SharedMemory/PhysicsClientExample.cpp | 10 ++++-- .../PhysicsClientSharedMemory.cpp | 9 ++++++ .../PhysicsServerCommandProcessor.cpp | 32 +++++++++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 7 ++++ examples/SharedMemory/SharedMemoryPublic.h | 3 ++ 7 files changed, 83 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index be95bab46..92c1692b0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1214,9 +1214,30 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd return cl->getJointInfo(bodyIndex, jointIndex, *info); } -int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info) +b3SharedMemoryCommandHandle b3GetDynamicInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) { - return 0; + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_GET_DYNAMIC_INFO; + command->m_getDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_getDynamicInfoArgs.m_linkIndex = linkIndex; + return (b3SharedMemoryCommandHandle) command; +} + +int b3GetDynamicInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicInfo* info) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + const b3DynamicInfo &dynamicInfo = status->m_dynamicInfo; + btAssert(status->m_type == CMD_GET_DYNAMIC_INFO); + if (status->m_type != CMD_GET_DYNAMIC_INFO_COMPLETED) + return false; + + info->m_mass = dynamicInfo.m_mass; + info->m_lateralFrictionCoeff = dynamicInfo.m_lateralFrictionCoeff; + return true; } b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 482c6f4d4..2d9b16630 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -77,8 +77,9 @@ 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); +b3SharedMemoryCommandHandle b3GetDynamicInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); ///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); +int b3GetDynamicInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicInfo* info); b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient); int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index b744a2d0b..86e03c0c9 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -54,8 +54,8 @@ protected: int m_canvasDepthIndex; int m_canvasSegMaskIndex; - float m_lightPos[3]; - float m_specularCoeff; + btScalar m_lightPos[3]; + btScalar m_specularCoeff; void createButton(const char* name, int id, bool isTrigger ); @@ -281,7 +281,11 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); - b3RequestCameraImageSetLightDirection(commandHandle, m_lightPos); + float lightPos[3]; + lightPos[0] = m_lightPos[0]; + lightPos[1] = m_lightPos[1]; + lightPos[2] = m_lightPos[2]; + b3RequestCameraImageSetLightDirection(commandHandle, lightPos); b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 27262685f..6c8bb1f83 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1039,6 +1039,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Removing body failed"); break; } + case CMD_GET_DYNAMIC_INFO_COMPLETED: + { + break; + } + case CMD_GET_DYNAMIC_INFO_FAILED: + { + b3Warning("Request dynamic info failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 30672dae5..6190cad6d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3955,6 +3955,38 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; + case CMD_GET_DYNAMIC_INFO: + { + int bodyUniqueId = clientCmd.m_getDynamicInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_getDynamicInfoArgs.m_linkIndex; + InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMIC_INFO_COMPLETED; + + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + serverCmd.m_dynamicInfo.m_mass = mb->getBaseMass(); + serverCmd.m_dynamicInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); + } + else + { + serverCmd.m_dynamicInfo.m_mass = mb->getLinkMass(linkIndex); + serverCmd.m_dynamicInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); + } + hasStatus = true; + } + else + { + b3Warning("The dynamic info requested is not available"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMIC_INFO_FAILED; + 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 5e339430a..b798d896f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -122,6 +122,12 @@ struct ResetDynamicInfoArgs double m_lateralFriction; }; +struct GetDynamicInfoArgs +{ + int m_bodyUniqueId; + int m_linkIndex; +}; + struct SetJointFeedbackArgs { int m_bodyUniqueId; @@ -739,6 +745,7 @@ struct SharedMemoryCommand struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; struct ResetDynamicInfoArgs m_resetDynamicInfoArgs; + struct GetDynamicInfoArgs m_getDynamicInfoArgs; 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 7602e309e..6a08a944a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -57,6 +57,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, CMD_REMOVE_BODY, CMD_RESET_DYNAMIC_INFO, + CMD_GET_DYNAMIC_INFO, CMD_PROFILE_TIMING, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -141,6 +142,8 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, CMD_REMOVE_BODY_COMPLETED, CMD_REMOVE_BODY_FAILED, + CMD_GET_DYNAMIC_INFO_COMPLETED, + CMD_GET_DYNAMIC_INFO_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; From 92de4ecd3144f7ee7865910f7d3da19d7b0b3c84 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 7 May 2017 22:41:05 -0700 Subject: [PATCH 3/5] Add pybullet example to get dynamic info. --- .../pybullet/examples/reset_dynamic_info.py | 4 ++ examples/pybullet/pybullet.c | 60 +++++++++++++++++++ 2 files changed, 64 insertions(+) diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py index 126312b07..06f02ef36 100644 --- a/examples/pybullet/examples/reset_dynamic_info.py +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -16,5 +16,9 @@ while 1: t=t+1 if t > 400: p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) + mass1,frictionCoeff1=p.getDynamicInfo(bodyUniqueId=0,linkIndex=-1) + mass2,frictionCoeff2=p.getDynamicInfo(bodyUniqueId=2,linkIndex=-1) + print mass1,frictionCoeff1 + print mass2,frictionCoeff2 time.sleep(.01) p.stepSimulation() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 037f5b181..c68a58975 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -647,6 +647,63 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj return Py_None; } +static PyObject* pybullet_getDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int bodyUniqueId = -1; + int linkIndex = -2; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getDynamicInfo failed; invalid bodyUniqueId"); + return NULL; + } + if (linkIndex < -1) + { + PyErr_SetString(SpamError, "getDynamicInfo failed; invalid linkIndex"); + return NULL; + } + cmd_handle = b3GetDynamicInfoCommandInit(sm, bodyUniqueId, linkIndex); + status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_GET_DYNAMIC_INFO_COMPLETED) + { + PyErr_SetString(SpamError, "getDynamicInfo failed; invalid return status"); + return NULL; + } + struct b3DynamicInfo info; + if (b3GetDynamicInfo(status_handle, &info)) + { + PyObject* pyDynamicInfo = PyTuple_New(2); + PyTuple_SetItem(pyDynamicInfo, 0, PyFloat_FromDouble(info.m_mass)); + PyTuple_SetItem(pyDynamicInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff)); + return pyDynamicInfo; + } + } + } + PyErr_SetString(SpamError, "Couldn't get dynamic info"); + return NULL; +} + + static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds) { double fixedTimeStep = -1; @@ -5584,6 +5641,9 @@ static PyMethodDef SpamMethods[] = { {"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS, "Reset dynamic information such as mass, lateral friction coefficient."}, + + {"getDynamicInfo", (PyCFunction)pybullet_getDynamicInfo, METH_VARARGS | METH_KEYWORDS, + "Get 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." From 98654a0cb433f1139e5a7b40446c55940a40f5b0 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 9 May 2017 10:31:28 -0700 Subject: [PATCH 4/5] Change dynamic to dynamics in dynamics info. --- examples/SharedMemory/PhysicsClientC_API.cpp | 48 +++++++++---------- examples/SharedMemory/PhysicsClientC_API.h | 10 ++-- .../SharedMemory/PhysicsClientExample.cpp | 2 +- .../PhysicsClientSharedMemory.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 38 +++++++-------- examples/SharedMemory/SharedMemoryCommands.h | 18 +++---- examples/SharedMemory/SharedMemoryPublic.h | 10 ++-- .../pybullet/examples/reset_dynamic_info.py | 15 +++--- examples/pybullet/pybullet.c | 42 ++++++++-------- 9 files changed, 93 insertions(+), 94 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 92c1692b0..aa1d3f3be 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1214,65 +1214,65 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd return cl->getJointInfo(bodyIndex, jointIndex, *info); } -b3SharedMemoryCommandHandle b3GetDynamicInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) +b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); - command->m_type = CMD_GET_DYNAMIC_INFO; - command->m_getDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; - command->m_getDynamicInfoArgs.m_linkIndex = linkIndex; + command->m_type = CMD_GET_DYNAMICS_INFO; + command->m_getDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_getDynamicsInfoArgs.m_linkIndex = linkIndex; return (b3SharedMemoryCommandHandle) command; } -int b3GetDynamicInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicInfo* info) +int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; - const b3DynamicInfo &dynamicInfo = status->m_dynamicInfo; - btAssert(status->m_type == CMD_GET_DYNAMIC_INFO); - if (status->m_type != CMD_GET_DYNAMIC_INFO_COMPLETED) + const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo; + btAssert(status->m_type == CMD_GET_DYNAMICS_INFO); + if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED) return false; - info->m_mass = dynamicInfo.m_mass; - info->m_lateralFrictionCoeff = dynamicInfo.m_lateralFrictionCoeff; + info->m_mass = dynamicsInfo.m_mass; + info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff; return true; } -b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3InitResetDynamicsInfo(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_type = CMD_RESET_DYNAMICS_INFO; command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; } -int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) +int b3ResetDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO); + b3Assert(command->m_type == CMD_RESET_DYNAMICS_INFO); b3Assert(mass > 0); - 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; + command->m_resetDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_resetDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_resetDynamicsInfoArgs.m_mass = mass; + command->m_updateFlags |= RESET_DYNAMICS_INFO_SET_MASS; return 0; } -int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) +int b3ResetDynamicsInfoSetLateralFriction(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; + b3Assert(command->m_type == CMD_RESET_DYNAMICS_INFO); + command->m_resetDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_resetDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_resetDynamicsInfoArgs.m_lateralFriction = lateralFriction; + command->m_updateFlags |= RESET_DYNAMICS_INFO_SET_LATERAL_FRICTION; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2d9b16630..58da884d4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -77,13 +77,13 @@ 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); -b3SharedMemoryCommandHandle b3GetDynamicInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); +b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); ///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h -int b3GetDynamicInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicInfo* info); +int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info); -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 b3InitResetDynamicsInfo(b3PhysicsClientHandle physClient); +int b3ResetDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); +int b3ResetDynamicsInfoSetLateralFriction(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/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 86e03c0c9..7fe0109d8 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -260,7 +260,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { #ifdef BT_DEBUG - b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); #else b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf");//two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf"); #endif diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 6c8bb1f83..c618e2afe 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1039,11 +1039,11 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Removing body failed"); break; } - case CMD_GET_DYNAMIC_INFO_COMPLETED: + case CMD_GET_DYNAMICS_INFO_COMPLETED: { break; } - case CMD_GET_DYNAMIC_INFO_FAILED: + case CMD_GET_DYNAMICS_INFO_FAILED: { b3Warning("Request dynamic info failed"); break; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6190cad6d..723ec9dfa 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3898,15 +3898,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; - case CMD_RESET_DYNAMIC_INFO: + case CMD_RESET_DYNAMICS_INFO: { - BT_PROFILE("CMD_RESET_DYNAMIC_INFO"); + BT_PROFILE("CMD_RESET_DYNAMICS_INFO"); - if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_MASS) + if (clientCmd.m_updateFlags & RESET_DYNAMICS_INFO_SET_MASS) { - int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex; - double mass = clientCmd.m_resetDynamicInfoArgs.m_mass; + int bodyUniqueId = clientCmd.m_resetDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_resetDynamicsInfoArgs.m_linkIndex; + double mass = clientCmd.m_resetDynamicsInfoArgs.m_mass; btAssert(bodyUniqueId >= 0); btAssert(linkIndex >= -1); @@ -3925,11 +3925,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION) + if (clientCmd.m_updateFlags & RESET_DYNAMICS_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; + int bodyUniqueId = clientCmd.m_resetDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_resetDynamicsInfoArgs.m_linkIndex; + double lateralFriction = clientCmd.m_resetDynamicsInfoArgs.m_lateralFriction; btAssert(bodyUniqueId >= 0); btAssert(linkIndex >= -1); @@ -3955,26 +3955,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; - case CMD_GET_DYNAMIC_INFO: + case CMD_GET_DYNAMICS_INFO: { - int bodyUniqueId = clientCmd.m_getDynamicInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_getDynamicInfoArgs.m_linkIndex; + int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMIC_INFO_COMPLETED; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; btMultiBody* mb = body->m_multiBody; if (linkIndex == -1) { - serverCmd.m_dynamicInfo.m_mass = mb->getBaseMass(); - serverCmd.m_dynamicInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); + serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass(); + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); } else { - serverCmd.m_dynamicInfo.m_mass = mb->getLinkMass(linkIndex); - serverCmd.m_dynamicInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); + serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex); + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); } hasStatus = true; } @@ -3982,7 +3982,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Warning("The dynamic info requested is not available"); SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMIC_INFO_FAILED; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; hasStatus = true; } break; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b798d896f..ac261af2d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -106,14 +106,14 @@ struct BulletDataStreamArgs char m_bodyName[MAX_FILENAME_LENGTH]; }; -enum EnumResetDynamicInfoFlags +enum EnumResetDynamicsInfoFlags { - RESET_DYNAMIC_INFO_SET_MASS=1, - RESET_DYNAMIC_INFO_SET_COM=2, - RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION=4, + RESET_DYNAMICS_INFO_SET_MASS=1, + RESET_DYNAMICS_INFO_SET_COM=2, + RESET_DYNAMICS_INFO_SET_LATERAL_FRICTION=4, }; -struct ResetDynamicInfoArgs +struct ResetDynamicsInfoArgs { int m_bodyUniqueId; int m_linkIndex; @@ -122,7 +122,7 @@ struct ResetDynamicInfoArgs double m_lateralFriction; }; -struct GetDynamicInfoArgs +struct GetDynamicsInfoArgs { int m_bodyUniqueId; int m_linkIndex; @@ -744,8 +744,8 @@ struct SharedMemoryCommand struct MjcfArgs m_mjcfArguments; struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; - struct ResetDynamicInfoArgs m_resetDynamicInfoArgs; - struct GetDynamicInfoArgs m_getDynamicInfoArgs; + struct ResetDynamicsInfoArgs m_resetDynamicsInfoArgs; + struct GetDynamicsInfoArgs m_getDynamicsInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; @@ -839,7 +839,7 @@ struct SharedMemoryStatus struct StateLoggingResultArgs m_stateLoggingResultArgs; struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs; struct b3ObjectArgs m_removeObjectArgs; - struct b3DynamicInfo m_dynamicInfo; + struct b3DynamicsInfo m_dynamicsInfo; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 6a08a944a..925106392 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -56,8 +56,8 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_KEYBOARD_EVENTS_DATA, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, CMD_REMOVE_BODY, - CMD_RESET_DYNAMIC_INFO, - CMD_GET_DYNAMIC_INFO, + CMD_RESET_DYNAMICS_INFO, + CMD_GET_DYNAMICS_INFO, CMD_PROFILE_TIMING, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -142,8 +142,8 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, CMD_REMOVE_BODY_COMPLETED, CMD_REMOVE_BODY_FAILED, - CMD_GET_DYNAMIC_INFO_COMPLETED, - CMD_GET_DYNAMIC_INFO_FAILED, + CMD_GET_DYNAMICS_INFO_COMPLETED, + CMD_GET_DYNAMICS_INFO_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -223,7 +223,7 @@ 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 +struct b3DynamicsInfo { double m_mass; double m_localInertialPosition[3]; diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py index 06f02ef36..ebf171275 100644 --- a/examples/pybullet/examples/reset_dynamic_info.py +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -3,21 +3,20 @@ import time import math p.connect(p.GUI) - -p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593]) +planeId = 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) +cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4]) +p.resetDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1) +#p.resetDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0) p.setGravity(0,0,-10) p.setRealTimeSimulation(0) t=0 while 1: t=t+1 if t > 400: - p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) - mass1,frictionCoeff1=p.getDynamicInfo(bodyUniqueId=0,linkIndex=-1) - mass2,frictionCoeff2=p.getDynamicInfo(bodyUniqueId=2,linkIndex=-1) + p.resetDynamicsInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) + mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1) + mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1) print mass1,frictionCoeff1 print mass2,frictionCoeff2 time.sleep(.01) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c68a58975..8e5b0b056 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -604,7 +604,7 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key return pylist; } -static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_resetDynamicsInfo(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId = -1; int linkIndex = -2; @@ -627,17 +627,17 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj } { - b3SharedMemoryCommandHandle command = b3InitResetDynamicInfo(sm); + b3SharedMemoryCommandHandle command = b3InitResetDynamicsInfo(sm); b3SharedMemoryStatusHandle statusHandle; if (mass >= 0) { - b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass); + b3ResetDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass); } if (lateralFriction >= 0) { - b3ResetDynamicInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); + b3ResetDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -647,7 +647,7 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj return Py_None; } -static PyObject* pybullet_getDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObject* keywds) { { int bodyUniqueId = -1; @@ -673,33 +673,33 @@ static PyObject* pybullet_getDynamicInfo(PyObject* self, PyObject* args, PyObjec if (bodyUniqueId < 0) { - PyErr_SetString(SpamError, "getDynamicInfo failed; invalid bodyUniqueId"); + PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid bodyUniqueId"); return NULL; } if (linkIndex < -1) { - PyErr_SetString(SpamError, "getDynamicInfo failed; invalid linkIndex"); + PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid linkIndex"); return NULL; } - cmd_handle = b3GetDynamicInfoCommandInit(sm, bodyUniqueId, linkIndex); + cmd_handle = b3GetDynamicsInfoCommandInit(sm, bodyUniqueId, linkIndex); status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); status_type = b3GetStatusType(status_handle); - if (status_type != CMD_GET_DYNAMIC_INFO_COMPLETED) + if (status_type != CMD_GET_DYNAMICS_INFO_COMPLETED) { - PyErr_SetString(SpamError, "getDynamicInfo failed; invalid return status"); + PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid return status"); return NULL; } - struct b3DynamicInfo info; - if (b3GetDynamicInfo(status_handle, &info)) + struct b3DynamicsInfo info; + if (b3GetDynamicsInfo(status_handle, &info)) { - PyObject* pyDynamicInfo = PyTuple_New(2); - PyTuple_SetItem(pyDynamicInfo, 0, PyFloat_FromDouble(info.m_mass)); - PyTuple_SetItem(pyDynamicInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff)); - return pyDynamicInfo; + PyObject* pyDynamicsInfo = PyTuple_New(2); + PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass)); + PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff)); + return pyDynamicsInfo; } } } - PyErr_SetString(SpamError, "Couldn't get dynamic info"); + PyErr_SetString(SpamError, "Couldn't get dynamics info"); return NULL; } @@ -5639,11 +5639,11 @@ static PyMethodDef SpamMethods[] = { "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, lateral friction coefficient."}, + {"resetDynamicsInfo", (PyCFunction)pybullet_resetDynamicsInfo, METH_VARARGS | METH_KEYWORDS, + "Reset dynamics information such as mass, lateral friction coefficient."}, - {"getDynamicInfo", (PyCFunction)pybullet_getDynamicInfo, METH_VARARGS | METH_KEYWORDS, - "Get dynamic information such as mass, lateral friction coefficient."}, + {"getDynamicsInfo", (PyCFunction)pybullet_getDynamicsInfo, METH_VARARGS | METH_KEYWORDS, + "Get dynamics information such as mass, lateral friction coefficient."}, {"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS, "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." From a587d4fec41babba9519ca841f94731d23f8c3ba Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 9 May 2017 10:44:33 -0700 Subject: [PATCH 5/5] Use "change" instead of "reset" for changing dynamics info. --- examples/SharedMemory/PhysicsClientC_API.cpp | 28 +++++++++---------- examples/SharedMemory/PhysicsClientC_API.h | 8 +++--- .../PhysicsClientSharedMemory.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 20 ++++++------- examples/SharedMemory/SharedMemoryCommands.h | 12 ++++---- examples/SharedMemory/SharedMemoryPublic.h | 2 +- .../pybullet/examples/reset_dynamic_info.py | 6 ++-- examples/pybullet/pybullet.c | 12 ++++---- 8 files changed, 45 insertions(+), 45 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index aa1d3f3be..5c235125f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1240,39 +1240,39 @@ int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3Dynamics return true; } -b3SharedMemoryCommandHandle b3InitResetDynamicsInfo(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); - command->m_type = CMD_RESET_DYNAMICS_INFO; + command->m_type = CMD_CHANGE_DYNAMICS_INFO; command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; } -int b3ResetDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) +int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command->m_type == CMD_RESET_DYNAMICS_INFO); + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); b3Assert(mass > 0); - command->m_resetDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; - command->m_resetDynamicsInfoArgs.m_linkIndex = linkIndex; - command->m_resetDynamicsInfoArgs.m_mass = mass; - command->m_updateFlags |= RESET_DYNAMICS_INFO_SET_MASS; + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_mass = mass; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MASS; return 0; } -int b3ResetDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) +int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command->m_type == CMD_RESET_DYNAMICS_INFO); - command->m_resetDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; - command->m_resetDynamicsInfoArgs.m_linkIndex = linkIndex; - command->m_resetDynamicsInfoArgs.m_lateralFriction = lateralFriction; - command->m_updateFlags |= RESET_DYNAMICS_INFO_SET_LATERAL_FRICTION; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_lateralFriction = lateralFriction; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 58da884d4..021bcf493 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -78,12 +78,12 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); -///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h +///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info); -b3SharedMemoryCommandHandle b3InitResetDynamicsInfo(b3PhysicsClientHandle physClient); -int b3ResetDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); -int b3ResetDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); +b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient); +int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); +int b3ChangeDynamicsInfoSetLateralFriction(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/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c618e2afe..2c1936e75 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1045,7 +1045,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } case CMD_GET_DYNAMICS_INFO_FAILED: { - b3Warning("Request dynamic info failed"); + b3Warning("Request dynamics info failed"); break; } default: { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 723ec9dfa..bd18e3cf7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3898,15 +3898,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; - case CMD_RESET_DYNAMICS_INFO: + case CMD_CHANGE_DYNAMICS_INFO: { - BT_PROFILE("CMD_RESET_DYNAMICS_INFO"); + BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO"); - if (clientCmd.m_updateFlags & RESET_DYNAMICS_INFO_SET_MASS) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { - int bodyUniqueId = clientCmd.m_resetDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_resetDynamicsInfoArgs.m_linkIndex; - double mass = clientCmd.m_resetDynamicsInfoArgs.m_mass; + int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; + double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; btAssert(bodyUniqueId >= 0); btAssert(linkIndex >= -1); @@ -3925,11 +3925,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - if (clientCmd.m_updateFlags & RESET_DYNAMICS_INFO_SET_LATERAL_FRICTION) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) { - int bodyUniqueId = clientCmd.m_resetDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_resetDynamicsInfoArgs.m_linkIndex; - double lateralFriction = clientCmd.m_resetDynamicsInfoArgs.m_lateralFriction; + int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; + double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; btAssert(bodyUniqueId >= 0); btAssert(linkIndex >= -1); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ac261af2d..e1a494fb8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -106,14 +106,14 @@ struct BulletDataStreamArgs char m_bodyName[MAX_FILENAME_LENGTH]; }; -enum EnumResetDynamicsInfoFlags +enum EnumChangeDynamicsInfoFlags { - RESET_DYNAMICS_INFO_SET_MASS=1, - RESET_DYNAMICS_INFO_SET_COM=2, - RESET_DYNAMICS_INFO_SET_LATERAL_FRICTION=4, + CHANGE_DYNAMICS_INFO_SET_MASS=1, + CHANGE_DYNAMICS_INFO_SET_COM=2, + CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION=4, }; -struct ResetDynamicsInfoArgs +struct ChangeDynamicsInfoArgs { int m_bodyUniqueId; int m_linkIndex; @@ -744,7 +744,7 @@ struct SharedMemoryCommand struct MjcfArgs m_mjcfArguments; struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; - struct ResetDynamicsInfoArgs m_resetDynamicsInfoArgs; + struct ChangeDynamicsInfoArgs m_changeDynamicsInfoArgs; struct GetDynamicsInfoArgs m_getDynamicsInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 925106392..9a373b115 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -56,7 +56,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_KEYBOARD_EVENTS_DATA, CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, CMD_REMOVE_BODY, - CMD_RESET_DYNAMICS_INFO, + CMD_CHANGE_DYNAMICS_INFO, CMD_GET_DYNAMICS_INFO, CMD_PROFILE_TIMING, //don't go beyond this command! diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py index ebf171275..0158f2e17 100644 --- a/examples/pybullet/examples/reset_dynamic_info.py +++ b/examples/pybullet/examples/reset_dynamic_info.py @@ -6,15 +6,15 @@ p.connect(p.GUI) planeId = 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]) cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4]) -p.resetDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1) -#p.resetDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0) +p.changeDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1) +#p.changeDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0) p.setGravity(0,0,-10) p.setRealTimeSimulation(0) t=0 while 1: t=t+1 if t > 400: - p.resetDynamicsInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) + p.changeDynamicsInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1) mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1) print mass1,frictionCoeff1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8e5b0b056..1b3cac5c4 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -604,7 +604,7 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key return pylist; } -static PyObject* pybullet_resetDynamicsInfo(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId = -1; int linkIndex = -2; @@ -627,17 +627,17 @@ static PyObject* pybullet_resetDynamicsInfo(PyObject* self, PyObject* args, PyOb } { - b3SharedMemoryCommandHandle command = b3InitResetDynamicsInfo(sm); + b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm); b3SharedMemoryStatusHandle statusHandle; if (mass >= 0) { - b3ResetDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass); + b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass); } if (lateralFriction >= 0) { - b3ResetDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); + b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -5639,8 +5639,8 @@ static PyMethodDef SpamMethods[] = { "Reset the state (position, velocity etc) for a joint on a body " "instantaneously, not through physics simulation."}, - {"resetDynamicsInfo", (PyCFunction)pybullet_resetDynamicsInfo, METH_VARARGS | METH_KEYWORDS, - "Reset dynamics information such as mass, lateral friction coefficient."}, + {"changeDynamicsInfo", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS, + "change dynamics information such as mass, lateral friction coefficient."}, {"getDynamicsInfo", (PyCFunction)pybullet_getDynamicsInfo, METH_VARARGS | METH_KEYWORDS, "Get dynamics information such as mass, lateral friction coefficient."},